﻿/***************************************************************************
 创建者: 华磊
 开始时间: 2016.11.28
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)

７）华磊，　2017.06.21。　重命名了一些函数，使含义更加清晰。
　　　　　　　　　　　　　因为修改了　MotorInterface\ internalSafe\ JointDecouple\ 接口，导致有相应修改。
                            　　　　　　增加了jointDecouple时的数据记录分析功能。
 ***************************************************************************
 *  @file mccontrollerinterpreter.cpp
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#include "mccontrollerinterpreter.h"
#include <math.h>
#include <unistd.h>
#include <kdl/frames_io.hpp>
#include "qdebug.h"
#include "timetool.h"
#include "TestUi/motionform.h"
#include "trajectoryserver.h"
#include "jogcontrol.h"
#include <qdir.h>
#include <QMainWindow>
#include "positionregulatorgroup.h"
#include "mathauthorityallowance.h"

//#define BETA

#define D_CHANGE_MAP_WAIT 8
#define RATIO_POWER 1.0e6
#define D_JOG_START_TIME 0.02
//2017.02.20 lfd改为 T1限速和坐标系数量在配置文件中读取
//#define T1_RATIO 0.1 //T1限制速度
//#define COORDINATE_SIZE 100 //先使用100

#ifndef MOTION_SERVER_UNIT_TEST
#include "robotcontrol.h"
#endif

using namespace KDL;
#ifdef D_USE_ROS
#include "rosapi.h"
//extern RosApi *g_rosApi;
#endif
extern QMainWindow *g_mainWindow;

McControllerInterpreter::McControllerInterpreter(int robotIdIn, RegData *regDataIn, RobotIo *ioControllerIn):
    robotId(robotIdIn)
{

    mcInterpreterRegData = regDataIn;
    ioController=ioControllerIn;
    versionInfo.componentName="MotionServer";
    versionInfo.componentVersion="V1.0";
    versionInfo.publishDate="2018.10.27";

    isInitialOk=false;

    rollerRatio = 0.1;//滚轮速度默认是10%
    isRollerFlag = false;//默认不使用滚轮
    controlType = ENUM_CONTROL_T1;//默认手动类型T1
    manualRatio = 0.2;//手动速率默认是20%
    autoRatio=0.05;
    status = ENUM_INTEPRETER_INIT;
    isrunning = false;
    debugFlag=0;

    isPalletInit = false;
    isServoParameterInitialed = 0;
    trajServer=NULL;
    robotAttribute=NULL;
    jogControl=NULL;

    pointData= new PointData();


    if(1==mcInit())
    {
        isInitialOk=true;

        //初始话设置默认motion model, 启用vehicle jog move
        QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
        QString infoFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/motionControl.ini";
        MC_MOTION_MODEL model;
        if(1!=robotAttribute->getMotionServerModel(infoFile,model))
        {
            if(debugFlag)
            {
                qDebug()<<"McControllerInterpreter::mcInit====getMotionServerModel error";
            }
            addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","McControllerInterpreter", 5302, robotId);
        }
        else
        {
            setMotionServerModel(model);
            startVehicleJogMove();
        }
    }
    else
    {
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","McControllerInterpreter", 5321, robotId);
    }

    #ifdef D_USE_UI
    //算法调试窗口
    motionForm = new MotionForm(this,robotId,robotAttribute->getAutoSaveWaveFlag());
//    motionForm->show();
    #endif

}
int McControllerInterpreter::getInitialStatus()
{
    return isInitialOk;
}

int McControllerInterpreter::showMotionForm()
{
    motionForm->showThisDialog();

    return 1;
}

int McControllerInterpreter::mcInit()
{
    //添加人：华磊　　添加内容：pointData实例以及regData实例
   // pointData = new PointData;
//    regData = new RegData;
//    if(debugFlag)
//    {
//        qDebug() << "McControllerInterpreter::mcInit====pointData:"<<pointData<<"====regData:"<<regData<<"\n\n\n";
//    }
    //axisInfo.xml
    initial_ros();

    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    QString xmlfile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/mcControllerInterpreter.xml";
    robotAttribute=new RobotAttribute(xmlfile,robotId,motionMessage);

    QString tmpIniFile=tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/runConfig.ini";
    double tmpAutoRatio,tmpJogRatio;
    int recordWareOut;
    if(1==robotAttribute->getAutoRatio(tmpIniFile,tmpAutoRatio,tmpJogRatio,recordWareOut))
    {
        autoRatio=tmpAutoRatio;
        manualRatio=tmpJogRatio;
    }

    if(0 > robotAttribute->getRobotAttribute(robotParameter))
    {
        if(debugFlag)
        {
            qDebug()<<"McControllerInterpreter::mcInit====读取机器人配置文件mcControllerInterpreter.xm.错误，初始化失败"<<status;
        }
        return -1;
    }


 //   rosClient= new RosClient("localhost:5051");

    t1RatioLimit=robotAttribute->t1RatioLimit;
    trajServer=new TrajectoryServer(&robotParameter,motionMessage,g_rosApi,mcInterpreterRegData,ioController,recordWareOut);
    motionMessage=new MotionMessage(trajServer);
    jogControl= new JogControl(trajServer, robotParameter,motionMessage);


    jointNum=robotParameter.jointNum;
    averageTorqueOffsetQueue.resize(jointNum);
//    analaysizerGravity = new DataAnalysizerDouble("analaysizerGravity",  0, 5000, 2*jointNum,10000, 2);
//    analysizerTorques = new DataAnalysizerDouble("analysizerTorques",  0, 5000, jointNum,10000, 2);
    jointDecouple=new JointDecouple(&robotParameter.jointAttributeVector,robotParameter.robotType
                                    ,robotId,deltaTime);
    toolCoordinateList=trajServer->getToolCoordinateList();
    userCoordinateList=trajServer->getUserCoordinateList();

//    palletCalculate = new PalletCalculate();

    //2017-5-18 添加
    //zeroMotorPosition.resize(jointNum);


    status = ENUM_TRAJECTORY_STOPPED;

    if(debugFlag)
    {
        qDebug()<<"McControllerInterpreter::mcInit====初始化结束"<<status;
    }

    deltaTime=trajServer->getDeltaTime();
    isUsePositionRegulatorStatus=robotAttribute->isUsePositionRegulator();
    if(1==isUsePositionRegulatorStatus)
    {
        QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
        QString xmlfile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/regulatorPositionConfig.xml";
        positionRegulators=new PositionRegulatorGroup(xmlfile,deltaTime,motionMessage,robotId,E_REGULATOR_TYPE_POSITION);
        velocityRegulators=NULL;
    }
    else if(2==isUsePositionRegulatorStatus)
    {
        QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
        QString xmlfile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/regulatorPositionConfig.xml";
        positionRegulators=new PositionRegulatorGroup(xmlfile,deltaTime,motionMessage,robotId,E_REGULATOR_TYPE_POSITION);
        xmlfile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/regulatorVelocityConfig.xml";
        velocityRegulators=new PositionRegulatorGroup(xmlfile,deltaTime,motionMessage,robotId,E_REGULATOR_TYPE_VELOCITY);
    }
    else
    {
        positionRegulators=NULL;
        velocityRegulators=NULL;
    }

//        initialServoParameter();
    #ifndef D_USE_ROS
    QString tmpPathName = D_TCRCONTROLLER_DATA_PATH;
    tmpPathName = tmpPathName+USER_INFO_MATH;

    QString licensePathName= D_TCRCONTROLLER_DATA_PATH;
    licensePathName = licensePathName+LICENSE_FILE_MATH;
    mathAuthority=new MathAuthorityAllowance(tmpPathName,licensePathName);
    int tmpReturn=checkRegisterStatus();
    if(ENUM_TIME_NORMAL!=tmpReturn)
    {
        return -1;
    }
    #else
    tmpIniFile=tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/runConfig.ini";

    PathMovingInfo pathMovingOut;
    VehiclePosition positionOut;
    if(1==robotAttribute->getLastFloorInfo(tmpIniFile,pathMovingOut,positionOut))
    {
        trajServer->setPathMovingInfo_virtualLineControl(pathMovingOut);
    }
    #endif
    return 1;
}


McControllerInterpreter::~McControllerInterpreter()
{
//    if(NULL!=pointData)
//    {
//        delete pointData;
//        pointData = NULL;
//    }
//    if(NULL!=mcInterpreterRegData)
//    {
//        delete mcInterpreterRegData;
//        mcInterpreterRegData = NULL;
//    }

    if(NULL!=jogControl)
    {
        delete jogControl;
    }
    if(NULL!=robotAttribute)
    {
        delete robotAttribute;
    }

    if(NULL!=trajServer)
    {
        delete trajServer;
    }
//    if(palletCalculate !=NULL)
//    {
//        delete palletCalculate;
//        palletCalculate = NULL;
//    }

//    if(NULL!=analaysizerGravity)
//    {
//        delete analaysizerGravity;
//        analaysizerGravity = NULL;
//    }
//    if(NULL!=analysizerTorques)
//    {
//        delete analysizerTorques;
//        analysizerTorques = NULL;
//    }

}

int McControllerInterpreter::getAxisSimpleTpInfo(int axisId, SimpleTpInfo &infoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getAxisSimpleTpInfo(axisId,infoOut);
}

int McControllerInterpreter::controlAxisSimpleTp(int axisId, int isEnable)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->controlAxisSimpleTp(axisId,isEnable);
}

int McControllerInterpreter::setAxisSimpleTpTarget(int axisId, double targetPosIn, double maxVelIn, double maxAccIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setAxisSimpleTpTarget(axisId,targetPosIn,maxVelIn,maxAccIn);
}

E_AXIS_PLAN_MODE McControllerInterpreter::getAxisPlanMode(int axisId)
{
    if(false==isInitialOk)
    {
        return E_AXIS_PLAN_MODE_ERROR;
    }
    return trajServer->getAxisPlanMode(axisId);
}

int McControllerInterpreter::setAxisPlanMode(int axisId, E_AXIS_PLAN_MODE modeIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setAxisPlanMode(axisId,modeIn);
}

int McControllerInterpreter::getArmMovingFlag(int &lineIdOut, E_MOVING_FLAG &extFlagOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getArmMovingFlag(lineIdOut,extFlagOut);
}

int McControllerInterpreter::addCircle(int idIn, std::vector<double> posIn, std::vector<double> centerIn,
                                       std::vector<double> normalIn, double vel,  int turnsIn, int velTypeIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->addCircle(idIn,posIn,centerIn,normalIn,vel,turnsIn,velTypeIn);
}

int McControllerInterpreter::resetAlgroCallTimeMeasure()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->resetAlgroCallTimeMeasure();
}

int McControllerInterpreter::getAlgroCallTimeMeasureDebug(vector<string> &comment, vector<double>& timeInfo)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getAlgroCallTimeMeasureDebug(comment,timeInfo);
}

int McControllerInterpreter::setTpNetFeedScale(double scaleIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setTpNetFeedScale(scaleIn);
}

int McControllerInterpreter::getRobotCommonPara(RobotCommonParameter &robotParameterOut)
{
    robotParameterOut=robotParameter;
    return 1;
}

int McControllerInterpreter::getEmcDebugInfo(EmcDebugInfo &debugOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getEmcDebugInfo(debugOut);
}

int McControllerInterpreter::isInCarvingMode()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->isInCarvingMode();
}

int McControllerInterpreter::setIsInCarvingMode(bool isIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setIsInCarvingMode(isIn);
}

int McControllerInterpreter::setCollisionWarnTorque(int axisIndex, double torqueIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setCollisionWarnTorque(axisIndex,torqueIn);
}

int McControllerInterpreter::setCollisionDetectRatio(double ratioIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setCollisionDetectRatio(ratioIn);
}

int McControllerInterpreter::setGravityVector(vector<double> gravityIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    robotAttribute->setGravityVector(gravityIn);
    return trajServer->setGravityVector(gravityIn);
}

int McControllerInterpreter::setToolMass(int toolIndex, double mass, vector<double> center)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setToolMass(toolIndex,mass,center);
}

int McControllerInterpreter::setSegmentAttribute(int index, JointAttribute attributeIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setSegmentAttribute(index,attributeIn);
}

int McControllerInterpreter::setDragPara(int axisIndex, DragPara paraIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setDragPara(axisIndex,paraIn);
}

int McControllerInterpreter::getTrigPosition(QQueue<PointPro> &trigPoseOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getTrigPosition(trigPoseOut);
}

int McControllerInterpreter::stopMotionTrig(int doIndex)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->stopMotionTrig(doIndex);
}

int McControllerInterpreter::setMotionTrigPara(int doIndex, double trigPeriod)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setMotionTrigPara(doIndex,trigPeriod);
}

int McControllerInterpreter::getReflectLaserFilterParameter(int index, LaserFilterParameter &paraOut)
{
    #ifdef D_USE_ROS
    return g_rosApi->getReflectLaserFilterParameter(index,paraOut);
    #endif
    return 0;
}

int McControllerInterpreter::setReflectLaserFilterParameter(int index, LaserFilterParameter paraIn)
{
    #ifdef D_USE_ROS
    return g_rosApi->setReflectLaserFilterParameter(index,paraIn);
    #endif
    return 0;
}

int McControllerInterpreter::getReflectCalculateCommonParameter(ReflectCommonDetectPara &paraOut)
{
    #ifdef D_USE_ROS
    return g_rosApi->getReflectCalculateCommonParameter(paraOut);
    #endif
    return 0;
}

int McControllerInterpreter::setReflectCalculateCommonParameter(ReflectCommonDetectPara paraIn)
{
    #ifdef D_USE_ROS
    return g_rosApi->setReflectCalculateCommonParameter(paraIn);
    #endif
    return 0;
}

int McControllerInterpreter::getAmclDebugInfo(AmclDebugInfo &debugInfoOut)
{
    #ifdef D_USE_ROS
    return g_rosApi->getAmclDebugInfo(debugInfoOut);
    #endif
    return 0;
}

int McControllerInterpreter::getVehicleMovingCaligrateInfo(VehicleMovingCaligrateInfo &infoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getVehicleMovingCaligrateInfo(infoOut);
}

int McControllerInterpreter::setCalibrateWheelDiameter(double idealMovedLength, double actualMovedLength)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setCalibrateWheelDiameter(idealMovedLength,actualMovedLength);
}

int McControllerInterpreter::setCalibrateWheelDistance(double idealMovedAngle, double actualMovedAngle)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setCalibrateWheelDistance(idealMovedAngle,actualMovedAngle);
}

int McControllerInterpreter::setAmclPara(AmclDebugPara paraIn)
{
    #ifdef D_USE_ROS
    return g_rosApi->setAmclPara(paraIn);
    #endif
    return 0;
}

int McControllerInterpreter::getAmclPara(AmclDebugPara &paraOut)
{
    #ifdef D_USE_ROS
    return g_rosApi->getAmclPara(paraOut);
    #endif
    return 0;
}

int McControllerInterpreter::getEncoderValue_belt(int beltIndexIn, int &encoderValueOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getEncoderValue_belt(beltIndexIn,encoderValueOut);
}

int McControllerInterpreter::getEncoderValue_vision(int visionIndexIn, int &encoderValueOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getEncoderValue_vision(visionIndexIn,encoderValueOut);
}

int McControllerInterpreter::forceConnectCamera(int visionIndex)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->forceConnectCamera(visionIndex);
}

int McControllerInterpreter::forceTrigCamera(int visionIndexIn,bool isHighIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->forceTrigCamera(visionIndexIn,isHighIn);
}

int McControllerInterpreter::getImuDeviceData(ImuDataInfo &dataOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getImuDeviceData(dataOut);
}

int McControllerInterpreter::setCoupleRelativeDistanceMonitorEnable(int followAxisId, bool isEnableIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setCoupleRelativeDistanceMonitorEnable(followAxisId,isEnableIn);
}

int McControllerInterpreter::getCoupleControlInfo(std::vector<CoupleConfigParameter> &infoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getCoupleControlInfo(infoOut);
}

int McControllerInterpreter::getAgvBorderInfo(VehicleBorderInfo &infoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getAgvBorderInfo(infoOut);
}

int McControllerInterpreter::setAgvBorderRectInfo(BorderRectInfo rectInfoIn, int &returnFlag)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setAgvBorderRectInfo(rectInfoIn,returnFlag);
}

int McControllerInterpreter::getVehicleKinematicDimension(VehicleKinematicDimension &infoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getVehicleKinematicDimension(infoOut);
}

int McControllerInterpreter::setVehicleKinematicDimension(VehicleKinematicDimension infoIn, int &returnFlag)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setVehicleKinematicDimension(infoIn,returnFlag);
}

int McControllerInterpreter::getLaserScanParameter(int laserId, LaserScanParameterHmi &infoOut)
{
    qDebug()<<"McControllerInterpreter::getLaserScanParameter begin";
    #ifdef D_USE_ROS
    return g_rosApi->getLaserScanParameter(laserId,infoOut);
    #endif
    return 0;
}

int McControllerInterpreter::setLaserScanParameter(int laserId, LaserScanParameterHmi infoIn, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setLaserScanParameter begin";
    #ifdef D_USE_ROS
    return g_rosApi->setLaserScanParameter(laserId,infoIn,returnFlag);
    #endif
    return 0;
}

int McControllerInterpreter::getVehicleJogControlParameter(VehicleJogControlParameter &infoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getVehicleJogControlParameter(infoOut);
}

int McControllerInterpreter::setVehicleJogControlParameter(VehicleJogControlParameter infoIn, int &returnFlag)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setVehicleJogControlParameter(infoIn,returnFlag);
}

int McControllerInterpreter::getVehicleTrajectoryControlParameter(VehicleTrajectoryControlParameter &infoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getVehicleTrajectoryControlParameter(infoOut);
}

int McControllerInterpreter::setVehicleTrajectoryControlParameter(VehicleTrajectoryControlParameter infoIn, int &returnFlag)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    if(1!=robotAttribute->setVehicleTrajectoryControlParameter(infoIn,returnFlag))
    {
        return -1;
    }
    return trajServer->setVehicleTrajectoryControlParameter(infoIn,returnFlag);
}

int McControllerInterpreter::setRotateEqradius(double rotate_eqradiusIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }

    return trajServer->setRotateEqradius(rotate_eqradiusIn);
}

//int McControllerInterpreter::setAxisPositionLimit(int axisId, double minPositionIn, double maxPositionIn,
//                                                  double approachDistanceIn)
//{
//    if(false==isInitialOk)
//    {
//        return -1;
//    }
//    int returnFlag;
//    if(1!=robotAttribute->setAxisPositionLimit(axisId,minPositionIn,maxPositionIn,approachDistanceIn,returnFlag))
//    {
//        return -1;
//    }
//    return trajServer->setAxisPositionLimit(axisId,minPositionIn,maxPositionIn,approachDistanceIn);
//}

int McControllerInterpreter::setAxisProperty(int axisId, AxisProperty_hmi axisPropertyIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }

    int tmpKey=robotAttribute->setAxisProperty(axisId,axisPropertyIn);
    if(1!=tmpKey)
    {
        qDebug()<<"error,,,robotAttribute->setAxisProperty,tmpKey "<<tmpKey;
        return -1;
    }
    return trajServer->setAxisProperty(axisId,axisPropertyIn);
}

int McControllerInterpreter::getAxisProperty(int axisId, AxisProperty_hmi &axisPropertyOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }

    return robotAttribute->getAxisProperty(axisId,axisPropertyOut);
}

int McControllerInterpreter::getRobotCoordinateConfig(RobotCoordinateAxisConfig &coordinateConfigOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }

    return robotAttribute->getRobotCoordinateConfig(coordinateConfigOut);
}

int McControllerInterpreter::setRobotCoordinateConfig(RobotCoordinateAxisConfig coordinateConfigIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }

    int tmpKey=robotAttribute->setRobotCoordinateConfig(coordinateConfigIn);
    if(1!=tmpKey)
    {
        qDebug()<<"error,,,robotAttribute->coordinateConfigIn,tmpKey "<<tmpKey;
        return -1;
    }
    t1RatioLimit=coordinateConfigIn.t1RatioLimit;
    jogControl->setRobotCoordinateConfig(coordinateConfigIn);
    return trajServer->setRobotCoordinateConfig(coordinateConfigIn);
}

int McControllerInterpreter::setCameraPointCoordinate(double x, double y, double z)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setCameraPointCoordinate(x,y,z);
}

int McControllerInterpreter::getCameraPointInBase(double &x, double &y, double &z)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getCameraPointInBase(x,y,z);
}

int McControllerInterpreter::getAgvSheftInfo(int sheftIndex,BorderRectInfo &infoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getAgvSheftInfo(sheftIndex,infoOut);
}

int McControllerInterpreter::setAgvSheftRectInfo(int sheftIndex,BorderRectInfo rectInfoIn, int &returnFlag)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setAgvSheftRectInfo(sheftIndex,rectInfoIn,returnFlag);
}

int McControllerInterpreter::getVehiclePositionCalibrateInfo(VehiclePosition &infoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getVehiclePositionCalibrateInfo(infoOut);
}

int McControllerInterpreter::getWeldPositionByCameraTransfer(const PointPro tcpPosition, double x, double y, double z, PointPro &pointOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getWeldPositionByCameraTransfer(tcpPosition,x,y,z,pointOut);
}

int McControllerInterpreter::calculateLaserCameraFrame(int indexIn, std::vector<PointPro> tcpPointList,
                                                       std::vector<PointPro> cameraPointList,
                                                       LaserCameraCalibrateResult &resultOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->calculateLaserCameraFrame(indexIn,tcpPointList,cameraPointList,resultOut);
}

int McControllerInterpreter::getLaserCameraFrameInfo(LaserCameraFrameInfo &infoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getLaserCameraFrameInfo(infoOut);
}

int McControllerInterpreter::setVelocityStyle(E_VELOCITY_TYPE velocityStypeIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setVelocityStyle(velocityStypeIn);
}

int McControllerInterpreter::setRobotArmLineMaxAcceleration(double accelerationIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setRobotArmLineMaxAcceleration(accelerationIn);
}

int McControllerInterpreter::setRobotArmLineMaxVelocity(double velIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setRobotArmLineMaxVelocity(velIn);
}

int McControllerInterpreter::setRobotArmJointMaxAcceleration(int axisIdIn, double accelerationIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setRobotArmJointMaxAcceleration(axisIdIn,accelerationIn);
}

int McControllerInterpreter::setRobotArmJointMaxVelocity(int axisIdIn, double velIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setRobotArmJointMaxVelocity(axisIdIn,velIn);
}

int McControllerInterpreter::activateCameraCoordinate(int index)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->activateCameraCoordinate(index);
}

int McControllerInterpreter::getCurrentCameraCoordinateIndex()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getCurrentCameraIndex();
}

int McControllerInterpreter::pointCoordinateToCartesian_cameraPoint(const PointPro jointPointIn,
                                       const PointPro cameraPointIn, PointPro &worldPointOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->pointCoordinateToCartesian_cameraPoint(jointPointIn,cameraPointIn,worldPointOut);
}

int McControllerInterpreter::calculateLaserCameraFrame(int indexIn, std::vector<PointPro> tcpPointList,
        std::vector<PointPro> cameraPointList, std::vector<float> &cameraTransferOut, double &maxErrorOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->calculateLaserCameraFrame(indexIn,tcpPointList,cameraPointList,cameraTransferOut,maxErrorOut);
}

int McControllerInterpreter::getVisionTrackConfigList(std::vector<VisionTrackConfig> &configListOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getVisionTrackConfigList(configListOut);
}

int McControllerInterpreter::getBeltTrackConfigList(std::vector<BeltTrackConfig> &configListOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getBeltTrackConfigList(configListOut);
}

int McControllerInterpreter::changeVisionTrackConfig(int id, VisionTrackConfig configIn, int &returnFlag)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->modifyVisionTrackConfig(id,configIn);
}

int McControllerInterpreter::changeBeltTrackConfig(int id, BeltTrackConfig configIn, int &returnFlag)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->modifyBeltTrackConfig(id,configIn);
}

int McControllerInterpreter::getMapPropertyInfo(string mapName, MapPropertyInfo &infoOut)
{
    qDebug()<<"McControllerInterpreter::getMapPropertyInfo begin";
    #ifdef D_USE_ROS
    return g_rosApi->getMapPropertyInfo(mapName,infoOut);
    #endif
    return 0;
}

int McControllerInterpreter::setWaveAcceleration(double accelerationIn,double maxVelIn,bool isRatio)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setWaveAcceleration(accelerationIn,maxVelIn,isRatio);
    return 1;
}

E_WAVE_STATUS McControllerInterpreter::getWaveMotionStatus()
{
    if(false==isInitialOk)
    {
        return E_WAVE_STATUS_STOPED;
    }
    return trajServer->getWaveMotionStatus();
}

int McControllerInterpreter::startWave()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->startWave();
}

int McControllerInterpreter::stopWave()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->stopWave();
}

int McControllerInterpreter::setWaveType(E_WAVE_TYPE typeIn, double waveWidthIn, double wavePeriodIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setWaveType(typeIn,waveWidthIn,wavePeriodIn);
}

int McControllerInterpreter::setWaveMotionInfo(WeaveMotionCondition infoIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setWaveMotionInfo(infoIn);
}

int McControllerInterpreter::setVisionTrigObjectLength(int visionIndex, double lengthIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setVisionTrigObjectLength(visionIndex,lengthIn);
}

int McControllerInterpreter::setTrackOffset(int beltIndex, double offset_x, double offset_y)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setTrackOffset(beltIndex,offset_x,offset_y);
}

int McControllerInterpreter::getPositionRegulatorDebugInfo(int regulatorIndex, RegulatorDebugInfo &infoOut)
{
    if(false==isInitialOk)
    {
        infoOut.axisId=-1000;
        return -1;
    }

    if(1==isUsePositionRegulatorStatus)
    {
        positionRegulators->getPositionRegulatorDebugInfo(regulatorIndex,infoOut);
        return 1;

    }
    else if(2==isUsePositionRegulatorStatus)
    {
        infoOut.axisId=-2000;
        return -1;


    }
    else
    {
        infoOut.axisId=-3000;
        return -1;
    }

    return 0;
}

int McControllerInterpreter::getMotorEncoderPulsePerRound(QVector<double> &pulseOut)
{
//    qDebug()<<"McControllerInterpreter::getMotorEncoderPulsePerRound";
    pulseOut.clear();
    for(int i=0;i<robotParameter.jointAttributeVector.size();i++)
    {
        pulseOut.append(robotParameter.jointAttributeVector[i].pulsePerRound);
    }
//    qDebug()<<"pulseOut"<<pulseOut;
    return 1;

}

int McControllerInterpreter::setEncoderReceiverOutput(unsigned short doIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setEncoderReceiverOutput(doIn);
}

int McControllerInterpreter::setVisionMatchKalmanFilterPara(int filterId, double followQ, double observeR)
{
    qDebug()<<"McControllerInterpreter::setVisionMatchKalmanFilterPara ";
    #ifdef D_USE_ROS
    g_rosApi->setVisionMatchKalmanFilterPara(filterId,followQ,observeR);
    #endif
    return 1;
}

int McControllerInterpreter::setSteeringControlKp(double kpIn, double maxSteerAdjustIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setSteeringControlKp(kpIn,maxSteerAdjustIn);
}

int McControllerInterpreter::setVehicleJogDebugFlag(int flagIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setVehicleJogDebugFlag(flagIn);
}

int McControllerInterpreter::setReflectMatchDebugFlag(int flagIn)
{
    qDebug()<<"McControllerInterpreter::setReflectMatchDebugFlag ";
    #ifdef D_USE_ROS
    g_rosApi->setReflectMatchDebugFlag(flagIn);
    #endif
    return 1;
}

int McControllerInterpreter::getAllReferenceMarks(std::vector<ReflectMarkInfo> &reflectMarksOut)
{
    qDebug()<<"McControllerInterpreter::getAllReferenceMarks ";
    #ifdef D_USE_ROS
    g_rosApi->getAllReferenceMarks(reflectMarksOut);
    #endif
    return 1;
}

int McControllerInterpreter::addReferenceMarkToMap(ReflectMarkInfo markIn)
{
    qDebug()<<"McControllerInterpreter::addReferenceMarkToMap ";
    #ifdef D_USE_ROS
    return g_rosApi->addReferenceMarkToMap(markIn);
    #endif
    return 1;
}

int McControllerInterpreter::saveReferenceMarkToFile(string mapFileName_yaml)
{
    qDebug()<<"McControllerInterpreter::saveReferenceMarkToFile ";
    #ifdef D_USE_ROS
    return g_rosApi->saveReferenceMarkToFile(mapFileName_yaml);
    #endif
    return 1;
}

int McControllerInterpreter::deleteReferenceMarkInMap(int uniqId)
{
    qDebug()<<"McControllerInterpreter::deleteReferenceMarkInMap uniqId=="<<uniqId;
    #ifdef D_USE_ROS
    g_rosApi->deleteReferenceMarkInMap(uniqId);
    #endif
    return 1;
}

int McControllerInterpreter::setRosDebugFlag(int flagIn)
{
    qDebug()<<"McControllerInterpreter::setRosDebugFlag flagIn=="<<flagIn;
    #ifdef D_USE_ROS
    g_rosApi->setDebugFlag(flagIn);
    #endif
    return 1;
}

int McControllerInterpreter::getCurrentPathInformation(VehiclePosition &startPositionOut, QVector<VehiclePosition> &targetPositionOut,
       int &pointIndexOut, E_NAV_COMMAND &currentPathTypeOut, double &currentPathParameter1Out, double &currentPathParameter2Out)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getCurrentPathInformation(startPositionOut,targetPositionOut,pointIndexOut,currentPathTypeOut,
                                                 currentPathParameter1Out,currentPathParameter2Out);
}

int McControllerInterpreter::resetCoupleOffset()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->resetCoupleOffset();
}

int McControllerInterpreter::getCoupleConfigInfo(int followAxisId, CoupleConfigParameter &coupleParaOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getCoupleConfigInfo(followAxisId,coupleParaOut);
}

int McControllerInterpreter::setAmclIsCalculateLaser(bool isCalculateLaserIn)
{
//        qDebug()<<"McControllerInterpreter::setAmclIsCalculateLaser isCalculateLaserIn=="<<isCalculateLaserIn;
        #ifdef D_USE_ROS
        g_rosApi->setAmclIsCalculateLaser(isCalculateLaserIn);
        #endif
        return 1;
}

int McControllerInterpreter::setCoupleControlEnable(int followAxisId, bool isEnableIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setCoupleControlEnable(followAxisId,isEnableIn);
}

int McControllerInterpreter::setCoupleControlOffset(int coupleId, double offfsetValueIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setCoupleControlOffset(coupleId,offfsetValueIn);
}

int McControllerInterpreter::getVisionTrackDebugInfo(int visionIndex, VisionTrackDebugInfo &visionInfoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getVisionTrackDebugInfo(visionIndex,visionInfoOut);
}

int McControllerInterpreter::getBeltTrackDebugInfo(int beltIndex, BeltTrackDebugInfo &beltTrackInfoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getBeltTrackDebugInfo(beltIndex,beltTrackInfoOut);
}

int McControllerInterpreter::modifyVisionTrackConfig(int visionIndex, VisionTrackConfig configIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->modifyVisionTrackConfig(visionIndex,configIn);
}

int McControllerInterpreter::modifyBeltTrackConfig(int beltIndex, BeltTrackConfig configIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->modifyBeltTrackConfig(beltIndex,configIn);
}

int McControllerInterpreter::runVisionTrack(int visionIndex)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->runVisionTrack(visionIndex);
}

int McControllerInterpreter::clearVisionData(int visionIndex)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->clearVisionData(visionIndex);
}

int McControllerInterpreter::trigVisionOnce(int visionIndex)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->trigVisionOnce(visionIndex);
}

int McControllerInterpreter::getVisionData(int visionIndex)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getVisionData(visionIndex);
}

int McControllerInterpreter::getTrackData(int beltIndex)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getTrackData(beltIndex);
}

int McControllerInterpreter::beltTrackStart(int beltIndex)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->beltTrackStart(beltIndex);
}

int McControllerInterpreter::beltTrackEnd(int beltIndex)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->beltTrackEnd(beltIndex);
}

int McControllerInterpreter::getEncoderReceiverData(std::vector<EncoderReceiverData> &dataListReturn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getEncoderReceiverData(dataListReturn);
}

E_BARRIER_RUN_MODE McControllerInterpreter::getBarrierRunMode()
{
    if(false==isInitialOk)
    {
        return E_BARRIER_RUN_MODE_NONE;
    }
    return trajServer->getBarrierRunMode();
}

int McControllerInterpreter::setBarrierRunMode(E_BARRIER_RUN_MODE modeIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setBarrierRunMode(modeIn);
}

int McControllerInterpreter::setPositionPid(int axisIdIn, double pid_p, double pid_i, double pid_d,
                                            double errorLimitForIntegration, double feedforward)
{
    if(NULL==positionRegulators)
    {
        return -1;
    }
    return positionRegulators->setPositionPid(axisIdIn,pid_p,pid_i,pid_d,errorLimitForIntegration,feedforward);
}

int McControllerInterpreter::setVelocityPid(int axisIdIn, double pid_p, double pid_i, double pid_d,
                                            double errorLimitForIntegration, double feedforward)
{
    if(NULL==velocityRegulators)
    {
        return -1;
    }
    return velocityRegulators->setPositionPid(axisIdIn,pid_p,pid_i,pid_d,errorLimitForIntegration,feedforward);
}

int McControllerInterpreter::getVehiclePositionType(E_VEHICLE_POSITION_TYPE &typeOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getVehiclePositionType(typeOut);
}

int McControllerInterpreter::loadMapAndSetOdometer( QString mapName, VehiclePosition position)
{
    #ifdef D_USE_ROS
    int returnFlag;
    // 加载地图
    loadMap(mapName.toStdString(),returnFlag);

    //根据地图的复杂程度，可能需要等待几秒种才能加载成功。
    sleep(D_CHANGE_MAP_WAIT);

    if(returnFlag != 1)
    {
        addMsg( ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter",  5311, robotId);
        return -1;
    }

    setVehiclePosition(position,D_SET_ODOMETER_COVE_RATIO,returnFlag);
    if(returnFlag != 1)
    {
        addMsg( ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter",  5312, robotId);
        return -1;
    }
    #endif
    return 1;
}

int McControllerInterpreter::getInternalLaserDriverStatus(QVector<int> &statusOut)
{
    //    qDebug()<<"McControllerInterpreter::getInternalLaserDriverStatus ";
        #ifdef D_USE_ROS
        return g_rosApi->getInternalLaserDriverStatus(statusOut);
        #endif
        return false;
}

int McControllerInterpreter::setQrcodeDetectParameter(QrcodeDetectParameter parameterIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setQrcodeDetectParameter(parameterIn);
}

int McControllerInterpreter::setQrcodeCameraId(int cameraIdIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setQrcodeCameraId(cameraIdIn);
}

int McControllerInterpreter::showQrcodeDebugWindow(int id)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->showQrcodeDebugWindow(id);

}

int McControllerInterpreter::getQrcodeDebugInfo(QrcodeDebugInfo &infoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getQrcodeSensorDebugInfo(infoOut);
}

int McControllerInterpreter::getCu1SensorRawData(int &requestCountOut, double &l1Out, double &l2Out,
                                                 double &imageOffsetOut, bool &isSucessOut, int &errorCodeOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getCu1SensorRawData(requestCountOut,l1Out,l2Out,imageOffsetOut,isSucessOut,errorCodeOut);
}

int McControllerInterpreter::setDebugFlag(int flagIn)
{
    debugFlag=flagIn;
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setDebugFlag(flagIn);
}

int McControllerInterpreter::setAgvPathFollowStrategy(E_PATH_CONTROL_STRATEGY stragetyIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setAgvPathFollowStrategy(stragetyIn);
}

int McControllerInterpreter::getBarriorDetectMode(E_BARRIER_TETECT_MODE &modeOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getBarriorDetectMode(modeOut);
}

int McControllerInterpreter::getSafeCollisionInfo(int sensorType, CollisionPara &infoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getSafeCollisionInfo(sensorType,infoOut);
}

int McControllerInterpreter::setSafeCollisionInfo(int sensorType, CollisionPara infoIn, int &returnFlag)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setSafeCollisionInfo(sensorType,infoIn,returnFlag);
}

bool McControllerInterpreter::isInCreatingMapStatus()
{
    //    qDebug()<<"McControllerInterpreter::isInCreatingMapStatus ";
        #ifdef D_USE_ROS
        return g_rosApi->isInCreatingMapStatus();
        #endif
        return false;
}

int McControllerInterpreter::setCurrentVehicleBorderId(int idIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setCurrentVehicleBorderId(idIn);
}

int McControllerInterpreter::getCurrentVehicleBorderId()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getCurrentVehicleBorderId();
}

int McControllerInterpreter::setLineFittingMethod(int methodIn)
{
    //    qDebug()<<"McControllerInterpreter::setLineFittingMethod ";
        #ifdef D_USE_ROS
        g_rosApi->setLineFittingMethod(methodIn);
        #endif
        return 1;
}

int McControllerInterpreter::setIsPrintVmarkProcessData(bool isPrint)
{
    //    qDebug()<<"McControllerInterpreter::setIsPrintVmarkProcessData ";
        #ifdef D_USE_ROS
        g_rosApi->setIsPrintVmarkProcessData(isPrint);
        #endif
        return 1;
}

int McControllerInterpreter::isVirtualLineTrajectoryEmpty()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->isVirtualLineTrajectoryEmpty();
}

int McControllerInterpreter::startMotorWave()
{
    motionForm->startMotorWave();
    return 1;
}

int McControllerInterpreter::stopMotorWave()
{
    motionForm->stopMotorWave();
    return 1;
}

int McControllerInterpreter::setOriginalLaserFilterType(E_LASER_FILTER_TYPE typeIn)
{
    //    qDebug()<<"McControllerInterpreter::setOriginalLaserFilterType ";
        #ifdef D_USE_ROS
        g_rosApi->setOriginalLaserFilterType(typeIn);
        #endif
        return 1;
}

int McControllerInterpreter::getCurrentOriginalLaserFilterType(E_LASER_FILTER_TYPE &typeOut)
{
    //    qDebug()<<"McControllerInterpreter::getCurrentOriginalLaserFilterType ";
        #ifdef D_USE_ROS
        g_rosApi->getCurrentOriginalLaserFilterType(typeOut);
        #endif
        return 1;
}

int McControllerInterpreter::setVirtualLineEnableKpChange(bool isEnable)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setVirtualLineEnableKpChange(isEnable);
}

int McControllerInterpreter::moveToNavgationPoint(MovingToNavagationParameter parameterIn, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::moveToNavgationPoint";
//    allStartRun();
//    //设置停车精度；距离0.02时，角度3度
//    setAgvTargetAllowOffset(0.2,0.5);
//    //设置是否检测障碍物
//    if(false==parameterIn.isDetectBarrier)
//    {
//        setUltrasonicSensorCheckEnable(false);
//        setPointCloudCheckEnable(false);
//    }
//    else
//    {
//        setUltrasonicSensorCheckEnable(true);
//        setPointCloudCheckEnable(true);
//    }
//    setMotionServerModel(ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK);
//    //odom set
////    int tmpReturn;
////    if(0==instructionIn.moveFreeMode)
////    {
////        programMcController->mcControllerInterpreter->setOdomSolver(E_ODOM_SOLVER_ENCODER,tmpReturn);
////    }
////    else if(2==instructionIn.moveFreeMode)
////    {
////        programMcController->mcControllerInterpreter->setOdomSolver(E_ODOM_SOLVER_LASER,tmpReturn);
////    }


////    programMcController->mcControllerInterpreter->startOneModel(ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK);

//    //vmark设置
//    if(EM_MOVE_FREE_DP==parameterIn.pointSensorType)
//    {
//        if(1!=changeToPositionType(E_VEHICLE_POSITION_TYPE_ROS))
//        {
//            addMsg( ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter",  5307, robotId);
//            returnFlag=-1;
//            allStop();
//            return returnFlag;
//        }
//        setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE_ROS,true);
////        setCheckVehiclePosition_vmark(false);
////        setCheckVehiclePosition_amcl(true);
//        setVirtualLineEnableKpChange(true);
//    }
//    else if(EM_MOVE_FREE_VMARK==parameterIn.pointSensorType)
//    {
//        //激光扫描范围
//        if(1!=setLaserIntensityFilterByType_visionMatch
//                (parameterIn.laserTypeNum))
//        {
//            addMsg( ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5305, robotId );
//            returnFlag=-2;
//            allStop();
//            return returnFlag;
//        }

//        setCurrentVMarkType(parameterIn.vmarkTypeNum);
//        if(1!=changeToPositionType(E_VEHICLE_POSITION_TYPE_VMARK))
//        {
//            addMsg( ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter",  5306, robotId );
//            returnFlag=-3;
//            allStop();
//            return returnFlag;
//        }
//        setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE_VMARK,true);
////        setCheckVehiclePosition_vmark(true);
////        setCheckVehiclePosition_amcl(false);
//        setVirtualLineEnableKpChange(false);
//    }
//    else if(EM_MOVE_FREE_QR==parameterIn.pointSensorType)
//    {
//        if(1!=changeToPositionType(E_VEHICLE_POSITION_TYPE_QRCODE))
//        {
//            addMsg( ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter",  5307, robotId);
//            returnFlag=-4;
//            allStop();
//            return returnFlag;
//        }
//        setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE_QRCODE,true);
////        setCheckVehiclePosition_vmark(false);
////        setCheckVehiclePosition_amcl(true);
//        setVirtualLineEnableKpChange(true);
//    }
//    else if(EM_MOVE_FREE_CU1==parameterIn.pointSensorType)
//    {
//        if(1!=changeToPositionType(E_VEHICLE_POSITION_TYPE_CU1))
//        {
//            addMsg( ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter",  5307, robotId);
//            returnFlag=-5;
//            allStop();
//            return returnFlag;
//        }
//        setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE_CU1,true);
////        setCheckVehiclePosition_vmark(false);
////        setCheckVehiclePosition_amcl(true);
//        setVirtualLineEnableKpChange(false);
//    }
//    else
//    {
//        addMsg( ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter",  5310, robotId);
//        returnFlag=-100;
//        allStop();
//        return returnFlag;
//    }

//    //添加轨迹指令
//    QVector<NavParameter> tmpCommandVector;
//    NavParameter tmpParameter;
//    tmpParameter.moveType=0;
//    tmpParameter.velocityUnit=ENUM_RATIO;
//    tmpParameter.velocity=parameterIn.speedRatio;
//    tmpParameter.targetPoint.posX=parameterIn.x;
//    tmpParameter.targetPoint.posY=parameterIn.y;
//    tmpParameter.targetPoint.posR=parameterIn.rotate;
//    tmpParameter.cntRatio=0;
//    tmpParameter.lineNumber=1;
//    tmpParameter.incFlag=0;
//    tmpParameter.pathFlag=0;
//    tmpParameter.isPositiveDirection=parameterIn.isPositiveDirection;
//    tmpCommandVector.append(tmpParameter);
//    addVirtualLineTrajectoryCommand(tmpCommandVector);
    returnFlag=1;

    return returnFlag;

}

int McControllerInterpreter::getVehicleSafeSpeedRatio(double &ratioOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getVehicleSafeSpeedRatio(ratioOut);
}

int McControllerInterpreter::setAgvTargetAllowOffset(float allowLineOffsetIn, float allowAngleOffsetIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setAgvTargetAllowOffset(allowLineOffsetIn,allowAngleOffsetIn);
}

int McControllerInterpreter::changeToPositionType(E_VEHICLE_POSITION_TYPE typeIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->changeToPositionType(typeIn);
}

int McControllerInterpreter::getVMarkPositionInWorld(VehiclePosition &positionOut, bool &isSucess)
{
    //    qDebug()<<"McControllerInterpreter::getVMarkPositionInWorld ";
        #ifdef D_USE_ROS
        g_rosApi->getVMarkPositionInWorld(positionOut,isSucess);
        #endif
        return 1;
}

int McControllerInterpreter::setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE typeIn, bool isCheck)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setCheckVehiclePosition(typeIn,isCheck);
}

int McControllerInterpreter::setCheckVehiclePosition_vmark(bool isCheck)
{
    //    qDebug()<<"McControllerInterpreter::setCheckVehiclePosition_vmark ";
        if(false==isInitialOk)
        {
            return -1;
        }
        return trajServer->setCheckVehiclePosition_vmark(isCheck);
}

int McControllerInterpreter::setCheckVehiclePosition_qrcode(bool isCheck)
{
    //    qDebug()<<"McControllerInterpreter::setCheckVehiclePosition_qrcode ";
        if(false==isInitialOk)
        {
            return -1;
        }
        return trajServer->setCheckVehiclePosition_qrcode(isCheck);
}

int McControllerInterpreter::setCheckVehiclePosition_amcl(bool isCheck)
{
    //    qDebug()<<"McControllerInterpreter::setCheckVehiclePosition_amcl ";
        if(false==isInitialOk)
        {
            return -1;
        }
        return trajServer->setCheckVehiclePosition_amcl(isCheck);
}

int McControllerInterpreter::setLaserIntensityFilterByType_visionMatch(int typeIn)
{
    //    qDebug()<<"McControllerInterpreter::setLaserIntensityFilterByType_visionMatch ";
        #ifdef D_USE_ROS
        g_rosApi->setLaserIntensityFilterByType_visionMatch(typeIn);
        #endif
        return 1;
}

int McControllerInterpreter::getMatchDebugInfo_vmark(VisionMatchDebugInfo &infoOut)
{
    //    qDebug()<<"McControllerInterpreter::getMatchDebugInfo_vmark ";
        #ifdef D_USE_ROS
        g_rosApi->getMatchDebugInfo_vmark(infoOut);
        #endif
        return 1;
}

int McControllerInterpreter::getMatchDebugInfo_reflect(ReflectDebugInfo &infoOut)
{
    //    qDebug()<<"McControllerInterpreter::getMatchDebugInfo_reflect ";
        #ifdef D_USE_ROS
        g_rosApi->getMatchDebugInfo_reflect(infoOut);
        #endif
        return 1;
}

int McControllerInterpreter::calculateVMarkPosition_byLaser(double baseLink_x, double baseLink_y, double baseLink_r,
                                                            vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
{
    //    qDebug()<<"McControllerInterpreter::calculateVMarkPosition_byLaser ";
        #ifdef D_USE_ROS
        g_rosApi->calculateVMarkPosition_byLaser(baseLink_x,baseLink_y,baseLink_r,x_out,y_out,r_out);
        #endif
        return 1;
}

int McControllerInterpreter::updateVMarkFile(QString fileName)
{
    //    qDebug()<<"McControllerInterpreter::updateVMarkFile ";
        #ifdef D_USE_ROS
        g_rosApi->updateVMarkFile(fileName);
        #endif
        return 1;
}

int McControllerInterpreter::createVMark(VMarkInfo infoIn, QString vmarkFileIn)
{
    //    qDebug()<<"McControllerInterpreter::createVMark ";
        #ifdef D_USE_ROS
        g_rosApi->createVMark(infoIn,vmarkFileIn);
        #endif
        return 1;
}

int McControllerInterpreter::getVmarkInfoList(QMap<int, VMarkInfo> &vmarkInfoOut)
{
    //    qDebug()<<"McControllerInterpreter::updateVMarkFile ";
        #ifdef D_USE_ROS
        g_rosApi->getVmarkInfoList(vmarkInfoOut);
        #endif
        return 1;
}

int McControllerInterpreter::setCurrentVMarkIndex(int indexIn)
{
    //    qDebug()<<"McControllerInterpreter::setCurrentVMarkIndex ";
        #ifdef D_USE_ROS
        g_rosApi->setCurrentVMarkIndex(indexIn);
        #endif
        return 1;
}

int McControllerInterpreter::setCurrentVMarkType(int currentVmarkTypeIn)
{
    //    qDebug()<<"McControllerInterpreter::setCurrentVMarkType ";
        #ifdef D_USE_ROS
        g_rosApi->setCurrentVMarkType(currentVmarkTypeIn);
        #endif
        return 1;
}

int McControllerInterpreter::detectVMark_baseMap(vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
{
    //    qDebug()<<"McControllerInterpreter::showDebugWindow_visionMatch ";
        #ifdef D_USE_ROS
        g_rosApi->detectVMark_baseMap(x_out,y_out,r_out);
        #endif
        return 1;
}

int McControllerInterpreter::detectVMark_laser(int vmark_index,vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
{
    //    qDebug()<<"McControllerInterpreter::showDebugWindow_visionMatch ";
        #ifdef D_USE_ROS
        g_rosApi->detectVMark_laser(vmark_index,x_out,y_out,r_out);
        #endif
        return 1;
}

int McControllerInterpreter::showDebugWindow_visionMatch(int id)
{
    //    qDebug()<<"McControllerInterpreter::showDebugWindow_visionMatch ";
        #ifdef D_USE_ROS
        g_rosApi->showDebugWindow_visionMatch(id);
        #endif
        return 1;
}

int McControllerInterpreter::setVmarkDetectParameter(VMarkDetectParameter vmarkParameterIn)
{
    //    qDebug()<<"McControllerInterpreter::setVmarkDetectParameter ";
        #ifdef D_USE_ROS
        g_rosApi->setVmarkDetectParameter(vmarkParameterIn);
        #endif
        return 1;
}

int McControllerInterpreter::getTimeConsumed_pointCloudDetect(double &minConsumedOut, double &currentConsumedOut, double &maxConsumedOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getTimeConsumed_pointCloudDetect(minConsumedOut,currentConsumedOut,maxConsumedOut);
}

int McControllerInterpreter::getTimeConsumed(int componentId, double &minConsumedOut, double &currentConsumedOut, double &maxConsumedOut)
{
    //    qDebug()<<"McControllerInterpreter::getTimeConsumed ";
        int returnFlag;
        #ifdef D_USE_ROS
        g_rosApi->getTimeConsumed(componentId,minConsumedOut,currentConsumedOut,maxConsumedOut);
        #endif
        return 1;
}

int McControllerInterpreter::resetRosMeasureTime()
{
    //    qDebug()<<"McControllerInterpreter::resetRosMeasureTime ";
        int returnFlag;
        #ifdef D_USE_ROS
        g_rosApi->resetRosMeasureTime();
        #endif
        return 1;
}

int McControllerInterpreter::setVirtualLinePid(double kpDistanceErrorIn, double kpAngleDistanceIn,
                                               double kpOnlyRotationIn, double kpDistanceError_yIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setVirtualLinePid(kpDistanceErrorIn,kpAngleDistanceIn,kpOnlyRotationIn,kpDistanceError_yIn);
}

int McControllerInterpreter::getTargetCommandVehiclePosition(VehiclePosition &targetOut, int &pointIndexOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getTargetCommandVehiclePosition(targetOut,pointIndexOut);
}

int McControllerInterpreter::getCarBorder(std::vector<VehiclePosition> &carBorderOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getCarBorder(carBorderOut);
}

int McControllerInterpreter::getCarBorderOffset(std::vector<VehiclePosition> &carBorderOffsetOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getCarBorderOffset(carBorderOffsetOut);
}

int McControllerInterpreter::getAllPointCloud_translated(std::vector<PointCloudData_3D> &pointCloudOut)
{
//    qDebug()<<"McControllerInterpreter::getAllPointCloud_translated ";
    int returnFlag;
    #ifdef D_USE_ROS
    g_rosApi->getAllPointCloud_translated(pointCloudOut);
    #endif
    return 1;
}

int McControllerInterpreter::setUltrasonicSensorCheckEnable(bool isOn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setUltrasonicSensorCheckEnable(isOn);
}

int McControllerInterpreter::getUltrasonicSensorSafeStatus(std::vector<double> &safeStatusOut, bool &isCheckEnableOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getUltrasonicSensorSafeStatus(safeStatusOut,isCheckEnableOut);
}

int McControllerInterpreter::getPointCloudSafeStatus(std::vector<string> &sensorNameOut, std::vector<double> &sensorSafeRatioOut,
                                                     bool &isCheckEnableOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getPointCloudSafeStatus(sensorNameOut,sensorSafeRatioOut,isCheckEnableOut);
}

int McControllerInterpreter::setPointCloudCheckEnable(bool isOn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setPointCloudCheckEnable(isOn);
}

int McControllerInterpreter::calibrateVehicle(double xOffset, double yOffset, double rOffset)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->calibrateVehicle(xOffset,yOffset,rOffset);
}

int McControllerInterpreter::getMapNameList(std::vector<string> &mapNameListOut)
{
    qDebug()<<"getMapNameList";

    QString path=D_MAP_PATH;

    QDir dir(path);
    qDebug()<<"path="<<dir;
    QStringList nameFilters;
    nameFilters << "*.yaml" ;
    QStringList files = dir.entryList(nameFilters, QDir::Files|QDir::Readable, QDir::Name);
    qDebug()<<"files="<<files;
    mapNameListOut.resize(files.size());
    for(int i=0;i<files.size();i++)
    {
        mapNameListOut[i]=files[i].toStdString();
    }
    return 1;
}

int McControllerInterpreter::getVehicleSpeedFeedback(VehicleVelocity &velocityOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getVehicleSpeedFeedback(velocityOut);
}

int McControllerInterpreter::resetCustomOdometer_positionManage()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->resetCustomOdometer_positionManage();
}

int McControllerInterpreter::getOdometer_positionManage(double &historyOdomOut, double &historyTimeOut, double &customOdomOut, double &customeTimeOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getOdometer_positionManage(historyOdomOut,historyTimeOut,customOdomOut,customeTimeOut);
}

int McControllerInterpreter::setVehicleAcceleration(double accelerationIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setVehicleAcceleration(accelerationIn);
}

int McControllerInterpreter::setVehicleAcceleration_rotate(double accelerationIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setVehicleAcceleration_rotate(accelerationIn);
}

int McControllerInterpreter::setVehiclePoseEstimation(double x, double y, double r)
{
    //    qDebug()<<"McControllerInterpreter::setVehiclePoseEstimation begin";
        int returnFlag;
        #ifdef D_USE_ROS
        g_rosApi->setVehiclePoseEstimation(x,y,r);
        #endif
        return 1;
}

int McControllerInterpreter::forceAmclUpdatePartial()
{
    //    qDebug()<<"McControllerInterpreter::forceAmclUpdatePartial begin";
        int returnFlag;
        #ifdef D_USE_ROS
        g_rosApi->forceAmclUpdatePartial();
        #endif
        return 1;
}

int McControllerInterpreter::setPclIcpWork(bool isWork)
{
    //    qDebug()<<"McControllerInterpreter::setPclIcpWork begin";
        int returnFlag;
        #ifdef D_USE_ROS
        UserInfo tmpUserInfo;
        g_rosApi->setPclIcpWork(isWork);
        #endif
        return 1;
}

int McControllerInterpreter::setVmarkWork(bool isWork)
{
    //    qDebug()<<"McControllerInterpreter::setVmarkWork begin";
        int returnFlag;
        #ifdef D_USE_ROS
        UserInfo tmpUserInfo;
        g_rosApi->setVmarkWork(isWork);
        #endif
        return 1;
}

int McControllerInterpreter::updatePclIcpDebugWindow()
{
    //    qDebug()<<"McControllerInterpreter::updatePclIcpDebugWindow begin";
        int returnFlag;
        #ifdef D_USE_ROS
        UserInfo tmpUserInfo;
        g_rosApi->updatePclIcpDebugWindow();
        #endif
        return 1;
}

int McControllerInterpreter::setMatchCalculateParameter(PclCalculationParameter paraIn)
{
    //    qDebug()<<"McControllerInterpreter::setMatchCalculateParameter begin";
        int returnFlag;
        #ifdef D_USE_ROS
        UserInfo tmpUserInfo;
        g_rosApi->setMatchCalculateParameter_pcl(paraIn);
        #endif
        return 1;
}

int McControllerInterpreter::getMatchDebugInfo(PclMatchDebugInfo &infoOut)
{
    //    qDebug()<<"McControllerInterpreter::getMatchDebugInfo begin";
        int returnFlag;
        #ifdef D_USE_ROS
        UserInfo tmpUserInfo;
        g_rosApi->getMatchDebugInfo_pcl(infoOut);
        #endif
        return 1;
}

int McControllerInterpreter::showVisionMatchDebugWindow(bool isShowBaseMap, bool isShowBaseCompareMap,
                                                        bool isShowLocalMap, bool isShowLocalCompareMap)
{
    //    qDebug()<<"McControllerInterpreter::showVisionMatchDebugWindow begin";
        int returnFlag;
        #ifdef D_USE_ROS
        UserInfo tmpUserInfo;
        g_rosApi->showVisionMatchDebugWindow_pcl(isShowBaseMap,isShowBaseCompareMap,isShowLocalMap,isShowLocalCompareMap);
        #endif
        return 1;
}

int McControllerInterpreter::getVehiclePosition_visionMatch(PositionInfo &positionInfoOut)
{
    //    qDebug()<<"McControllerInterpreter::getVehiclePosition_visionMatch begin";
        int returnFlag;
        #ifdef D_USE_ROS
        UserInfo tmpUserInfo;
        g_rosApi->getVehiclePosition_pclicp(tmpUserInfo,positionInfoOut);
        #endif
        return 1;
}

int McControllerInterpreter::calculateVisionMatchVehiclePosition(MapRegion baseRegionIn, MapRegion localRegionIn)
{
    //    qDebug()<<"McControllerInterpreter::calculateVisionMatchVehiclePosition begin";
        int returnFlag;
        #ifdef D_USE_ROS
        g_rosApi->calculateVehiclePosition_pcl(baseRegionIn,localRegionIn);
        #endif
        return 1;
}

int McControllerInterpreter::setBaseMapRegion_visionMatch(MapRegion baseRegionIn)
{
    //    qDebug()<<"McControllerInterpreter::setBaseMapRegion_visionMatch begin";
        int returnFlag;
        #ifdef D_USE_ROS
        g_rosApi->setBaseMapRegion_visionMatch(baseRegionIn);
        #endif
        return 1;
}

int McControllerInterpreter::setLocalMapRegion_visionMatch(MapRegion localRegionIn)
{
    //    qDebug()<<"McControllerInterpreter::setLocalMapRegion_visionMatch begin";
        int returnFlag;
        #ifdef D_USE_ROS
        g_rosApi->setLocalMapRegion_visionMatch(localRegionIn);
        #endif
        return 1;
}

int McControllerInterpreter::setLocalMapLaserRegion_visionMatch(double minumIntensityIn, double maxLaserRangeIn,
                                                                double minAngleIn, double maxAngleIn)
{
    //    qDebug()<<"McControllerInterpreter::setLocalMapLaserRegion_visionMatch begin";
        int returnFlag;
        #ifdef D_USE_ROS
        g_rosApi->setLocalMapLaserRegion_visionMatch(minumIntensityIn,maxLaserRangeIn,minAngleIn,maxAngleIn);
        #endif
        return 1;
}

int McControllerInterpreter::setBaseMapRegion_pclicp(MapRegion baseRegionIn)
{
    //    qDebug()<<"McControllerInterpreter::setBaseMapRegion begin";
        int returnFlag;
        #ifdef D_USE_ROS
        g_rosApi->setBaseMapRegion_pcl(baseRegionIn);
        #endif
        return 1;
}

int McControllerInterpreter::setLocalMapRegion_pclicp(MapRegion localRegionIn)
{
    //    qDebug()<<"McControllerInterpreter::setLocalMapRegion begin";
        int returnFlag;
        #ifdef D_USE_ROS
        g_rosApi->setLocalMapRegion_pcl(localRegionIn);
        #endif
        return 1;
}

int McControllerInterpreter::setLaserIntensityFilter(double minumIntensityIn,double maxLaserRangeIn,
                                                     double minAngleIn, double maxAngleIn)
{
    //    qDebug()<<"McControllerInterpreter::setLaserIntensityFilter begin";
        int returnFlag;
        #ifdef D_USE_ROS
        g_rosApi->setLaserIntensityFilter(minumIntensityIn,maxLaserRangeIn,minAngleIn,maxAngleIn);
        #endif
        return 1;
}

int McControllerInterpreter::getVehicleFollowOffset(double &currentLineOffsetOut, double &currentAngleOffsetOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getVehicleFollowOffset(currentLineOffsetOut,currentAngleOffsetOut);
}


int McControllerInterpreter::getTrackPidOutput(double &anglePidOutput, double &xPidOutput, double &yPidOutput)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getTrackPidOutput(anglePidOutput,xPidOutput,yPidOutput);
}

int McControllerInterpreter::addVirtualLineTrajectoryCommand(QVector<NavParameter> commandVectorIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->addVirtualLineTrajectoryCommand(commandVectorIn);
}

int McControllerInterpreter::setMaganeticInputIndex(int indexIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setMaganeticInputIndex(indexIn);
}

int McControllerInterpreter::writeTagData(int aliasIn, QVector<unsigned char> cardValueIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->writeTagData(aliasIn,cardValueIn);
}

int McControllerInterpreter::writeTagData(int aliasIn, long long cardValueIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->writeTagData(aliasIn,cardValueIn);
}

int McControllerInterpreter::getCurrentTagData(int aliasIn, long long &cardValueOut, bool &isCardDetected)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getCurrentTagData(aliasIn,cardValueOut,isCardDetected);
}

int McControllerInterpreter::getCurrentTagData(int aliasIn, QVector<unsigned char> &cardValueOut, bool &isCardDetected)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getCurrentTagData(aliasIn,cardValueOut,isCardDetected);
}

int McControllerInterpreter::getMaganeticSensorRawValue(int aliasIn,QVector<bool> &sensorRawValueOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getMaganeticSensorRawValue(aliasIn,sensorRawValueOut);
}

int McControllerInterpreter::getVehicleVelCommand(bool isJogCommand,double &xMove, double &yMove, double &rotate)
{
    return trajServer->getVehicleVelCommand(isJogCommand,xMove,yMove,rotate);
}

int McControllerInterpreter::getFrontLaserData(LaserDataInfo &frontLaserDataOut)
{
    //    qDebug()<<"McControllerInterpreter::getFrontLaserData begin";
        int returnFlag;
        #ifdef D_USE_ROS
    //    g_rosApi->startNavigation(userInfo,returnFlag);
        g_rosApi->getFrontLaserData(frontLaserDataOut);
        #endif
        return 1;
}

int McControllerInterpreter::getAmclMatchScore(double &mapDistance_out,float &weight_out, std::vector<float> &pos_cov_out)
{
//    qDebug()<<"McControllerInterpreter::getAmclMatchScore begin";
    int returnFlag;
    #ifdef D_USE_ROS
//    g_rosApi->startNavigation(userInfo,returnFlag);
    g_rosApi->getAmclMatchScore(mapDistance_out,weight_out,pos_cov_out);
    #endif
    return 1;
}

int McControllerInterpreter::startNavigation()
{
    qDebug()<<"McControllerInterpreter::startNavigation begin";
    int returnFlag;
    #ifdef D_USE_ROS
//    g_rosApi->startNavigation(userInfo,returnFlag);
    g_rosApi->startNavigation_bySignal();
    #endif
    return 1;
}

int McControllerInterpreter::stopNavigation()
{
    qDebug()<<"McControllerInterpreter::stopNavigation begin";
    int returnFlag;
    #ifdef D_USE_ROS
    g_rosApi->stopNavigation(userInfo,returnFlag);
    #endif
    return 1;
}

int McControllerInterpreter::getCurrentMapName(string &mapNameReturn)
{
//    qDebug()<<"McControllerInterpreter::getCurrentMapName ";
    int returnFlag;
    #ifdef D_USE_ROS
    g_rosApi->getCurrentMapName(mapNameReturn);
    #endif
    return 1;
}



int McControllerInterpreter::setMapSolver( E_MAP_SOLVER solverIn, int laserCount, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setMapSolver begin";
    #ifdef D_USE_ROS
    g_rosApi->setMapSolver(userInfo,solverIn,laserCount,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::createMapStart( int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::createMapStart begin";
    if(ENUM_CONTROL_T1 != controlType && ENUM_CONTROL_T2 != controlType)
    {
        qDebug()<<"current controlType="<<controlType;
        addMsg( ENUM_MSG_WARNING, "McControllerInterpreter", "McControllerInterpreter", 5309, robotId );
        returnFlag=-1;
        return -1;
    }
    setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE_ROS,false);
    #ifdef D_USE_ROS
    g_rosApi->createMapStart(userInfo,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::createMapEnd( string mapName, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::createMapEnd begin";
    if(ENUM_CONTROL_T1 != controlType && ENUM_CONTROL_T2 != controlType)
    {
        qDebug()<<"current controlType="<<controlType;
        addMsg( ENUM_MSG_WARNING, "McControllerInterpreter", "McControllerInterpreter", 5309, robotId );
        returnFlag=-1;
        return -1;
    }
    #ifdef D_USE_ROS
    g_rosApi->createMapEnd(userInfo,mapName,returnFlag);
    #endif
    setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE_ROS,true);
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::loadMap( string mapName, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::loadMap begin";
    PathMovingInfo pathInfo;
    pathInfo.currentFloorId=-1;
    pathInfo.passedStationId=-1;
    pathInfo.nextStationId=-1;
    pathInfo.currentStationId=-1;
    pathInfo.passedPathId=-1;
    pathInfo.nextPathId=-1;
    pathInfo.currentPathId=-1;
    setPathMovingInfo_virtualLineControl(pathInfo);//切换地图后需要重置信息.
    #ifdef D_USE_ROS
    g_rosApi->loadMap(userInfo,mapName,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::setDefaultMapName(string fileNameIn, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setDefaultMapName begin";
    #ifdef D_USE_ROS
    g_rosApi->setDefaultMapName(fileNameIn,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::addVitrualBlock( string configFilePath, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::addVitrualBlock begin";
    #ifdef D_USE_ROS
    g_rosApi->addVitrualBlock(userInfo,configFilePath,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::deleteVitrualBlock( int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::deleteVitrualBlock begin";
    #ifdef D_USE_ROS
    g_rosApi->deleteVitrualBlock(userInfo,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}



int McControllerInterpreter::addForbidenZone( double x_start, double y_start, double x_end, double y_end, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::addForbidenZone begin";
    #ifdef D_USE_ROS
//    g_rosApi->addForbidenZone(userInfo,x_start,y_start,x_end,y_end,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::setOdomSolver( E_ODOM_SOLVER solverIn, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setOdomSolver begin";
    #ifdef D_USE_ROS
    g_rosApi->setOdomSolver(userInfo,solverIn,returnFlag);
    #endif
     return 0;//必须有，代表通信成功

}

int McControllerInterpreter::setLocationSolver( E_LOCATION_SOLVER solverIn,
                                                int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setLocationSolver begin";
    #ifdef D_USE_ROS
    g_rosApi->setLocationSolver(userInfo,solverIn,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::getVehiclePosition( VehiclePosition &positionReturn)
{
//    qDebug()<<"McControllerInterpreter::getVehiclePosition begin";
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getVehiclePosition(positionReturn);
}

int McControllerInterpreter::getVehiclePosition_specified(PositionInfo &positionReturn, E_VEHICLE_POSITION_TYPE typeIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getVehiclePosition_specified(positionReturn,typeIn);
}

int McControllerInterpreter::setVehiclePosition( VehiclePosition positionIn, double coveRatioIn,int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setVehiclePosition begin";
    if(false==isInitialOk)
    {
        return -1;
    }
    //导致ｖirtualLine无法工作。
//    trajServer->setMotionServerModel(ENUM_MOTION_MODEL_VEHICLE_ROS_CONTROL);
    trajServer->startVehicleJogMove();
    #ifdef D_USE_ROS
    g_rosApi->setVehiclePosition(userInfo,positionIn,coveRatioIn,returnFlag);
    #endif
    trajServer->initialVehiclePosition(E_VEHICLE_POSITION_TYPE_ROS);
    trajServer->initialVehiclePosition(E_VEHICLE_POSITION_TYPE_CU1);
     return 0;//必须有，代表通信成功

}

int McControllerInterpreter::setReplanCoutLimitWhenFailure( int countLimit, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setReplanCoutLimitWhenFailure begin";
    #ifdef D_USE_ROS
    g_rosApi->setReplanCoutLimitWhenFailure(userInfo,countLimit,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::setTrajectoryGlobalPlanSolver( E_TRAJECTORY_GLOBAL_SOLVER solverIn, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setTrajectoryGlobalPlanSolver begin";
    #ifdef D_USE_ROS
    g_rosApi->setTrajectoryGlobalPlanSolver(userInfo,solverIn,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::setTrajectoryLocalPlanSolver( E_TRAJECTORY_LOCAL_SOLVER solverIn, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setTrajectoryLocalPlanSolver begin";
    #ifdef D_USE_ROS
    g_rosApi->setTrajectoryLocalPlanSolver(userInfo,solverIn,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::moveToPosition( VehiclePosition positionIn, double velocity, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::moveToPosition begin";
    trajServer->setMotionServerModel(ENUM_MOTION_MODEL_VEHICLE_ROS_CONTROL);
    trajServer->startVehicleJogMove();
    UserInfo tmpUser;

    #ifdef D_USE_ROS
//    g_rosApi->moveToPosition_bySignal(positionIn,velocity);
    g_rosApi->moveToPosition(tmpUser,positionIn,velocity,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::moveFollowPath( std::vector<VehiclePosition> pathIn,
                                             double velocity, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::moveFollowPath begin";
    trajServer->setMotionServerModel(ENUM_MOTION_MODEL_VEHICLE_ROS_CONTROL);
    trajServer->startVehicleJogMove();
    #ifdef D_USE_ROS
//    g_rosApi->moveFollowPath(userInfo,pathIn,velocity,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::changeVelocity( double velocity, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::changeVelocity begin";
    #ifdef D_USE_ROS
    g_rosApi->changeVelocity(userInfo,velocity,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::getMovingStatus( E_GOAL_STATUS &status)
{
//    qDebug()<<"McControllerInterpreter::getMovingStatus begin";
    #ifdef D_USE_ROS
    g_rosApi->getMovingStatus(userInfo,status);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::getRemainningTime( double &timeOut)
{
    qDebug()<<"McControllerInterpreter::getRemainningTime begin";
    #ifdef D_USE_ROS
    g_rosApi->getRemainningTime(userInfo,timeOut);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::getRemainningDistance( double &distanceOut)
{
    qDebug()<<"McControllerInterpreter::getRemainningDistance begin";
    #ifdef D_USE_ROS
    g_rosApi->getRemainningDistance(userInfo,distanceOut);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::ceaseMotion( int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::ceaseMotion begin";
    #ifdef D_USE_ROS
    g_rosApi->ceaseMotion(userInfo,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::startMotion( int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::startMotion begin";
    #ifdef D_USE_ROS
    g_rosApi->startMotion(userInfo,returnFlag);
    #endif
    trajServer->startVehicleJogMove();
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::cancelMotion( int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::cancelMotion begin";
    #ifdef D_USE_ROS
    g_rosApi->cancelMotion(userInfo,returnFlag);
    #endif
    return 0;//必须有，代表通信成功

}

int McControllerInterpreter::setPositionArrivedAccuracy( double accuracyIn, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setPositionArrivedAccuracy begin";
    #ifdef D_USE_ROS
    g_rosApi->setPositionArrivedAccuracy(userInfo,accuracyIn,accuracyIn,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::setLocalPlanRange( double rangeIn, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setLocalPlanRange begin";
    #ifdef D_USE_ROS
    g_rosApi->setLocalPlanRange(userInfo,rangeIn,rangeIn,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::setVehicleBorderOffset( std::vector<double> point_x, std::vector<double> point_y, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setVehicleBorderOffset begin";
    #ifdef D_USE_ROS
//    g_rosApi->setVehicleBorderOffset(userInfo,point_x,point_y,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::setVehicleBorderOffset(double front_offsetIn, double back_offsetIn, double left_offsetIn,
                                                    double right_offsetIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setVehicleBorderOffset(front_offsetIn,back_offsetIn,left_offsetIn,right_offsetIn);
}

int McControllerInterpreter::setGlobalBarrierOffset( double offsetIn, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setGlobalBarrierOffset begin";
    #ifdef D_USE_ROS
    g_rosApi->setGlobalBarrierOffset(userInfo,offsetIn,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}
int McControllerInterpreter::setLocalBarrierOffset( double offsetIn, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setLocalBarrierOffset begin";
    #ifdef D_USE_ROS
    g_rosApi->setLocalBarrierOffset(userInfo,offsetIn,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::resetRosAll( int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::resetRosAll begin";
    #ifdef D_USE_ROS
//    g_rosApi->resetRosAll(userInfo,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::getRosPackageStatus(E_ROS_PACKAGE package, E_ROS_PACKAGE_STATUS &status)
{
    qDebug()<<"McControllerInterpreter::getRosPackageStatus begin";
    #ifdef D_USE_ROS
//    g_rosApi->getRosPackageStatus(userInfo,package,status);
    #endif

    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::startImuCalibrate( int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::startImuCalibrate begin";
    #ifdef D_USE_ROS
//    g_rosApi->startImuCalibrate(userInfo,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::stopImuCalibrate( int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::stopImuCalibrate begin";
    #ifdef D_USE_ROS
//    g_rosApi->stopImuCalibrate(userInfo,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::getMessage(std::vector<Message> &messageListReturn)
{
    qDebug()<<"McControllerInterpreter::getMessage begin";
    #ifdef D_USE_ROS
//    g_rosApi->getMessage_hmi(messageListReturn);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::setWheelOdometerData(int robotIdIn, OdemeterData odemeterDataReturn, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setWheelOdometerData begin";
    #ifdef D_USE_ROS
    g_rosApi->setWheelOdometerData(odemeterDataReturn);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::getVehicleVelocityComand(int robotIdIn, VehicleVelocity &velocityComamnd)
{
    qDebug()<<"McControllerInterpreter::getVehicleVelocityComand begin";
    #ifdef D_USE_ROS
    g_rosApi->getVehicleVelocityComand(userInfo,robotIdIn,velocityComamnd);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::setWheelMoveSpeed(int robotIdIn, std::vector<double> wheelMoveSpeed, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setWheelMoveSpeed begin";
    #ifdef D_USE_ROS
//    g_rosApi->setWheelMoveSpeed(robotIdIn,wheelMoveSpeed,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::setWheelPathLength(int robotIdIn, std::vector<double> wheelPathLength, int &returnFlag)
{
    qDebug()<<"McControllerInterpreter::setWheelPathLength begin";
    #ifdef D_USE_ROS
//    g_rosApi->setWheelPathLength(robotIdIn,wheelPathLength,returnFlag);
    #endif
    return 0;//必须有，代表通信成功
}

int McControllerInterpreter::startGoZero_forUser(int axisId)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->startGoZero_forUser(axisId);
}

int McControllerInterpreter::startGoZero_forUser()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->startGoZero_forUser();
}

int McControllerInterpreter::haltGoZero_forUser()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->haltGoZero_forUser();
}

bool McControllerInterpreter::isAbsoulteMotorBatteryOk()
{
    if(false==isInitialOk)
    {
        return false;
    }
    return trajServer->isAbsoulteMotorBatteryOk();
}

int McControllerInterpreter::allowSingleAxisJog(bool enable)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return jogControl->allowSingleAxisJog(enable);
}

int McControllerInterpreter::getDebugInfo(MotionDebugInfo &infoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getDebugInfo(infoOut);
}

int McControllerInterpreter::getVirtuleLineDebugInfo(VirtualLineDebugInfo &infoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getVirtuleLineDebugInfo(infoOut);
}

int McControllerInterpreter::setSafeSpeedRatio(double safeSpeedRatioIn, double time)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setSafeSpeedRatio(safeSpeedRatioIn,time);
}

int McControllerInterpreter::controllerShutDown()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->controllerShutDown();

    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    QString tmpIniFile=tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/runConfig.ini";
    robotAttribute->writeAutoRatio(tmpIniFile,autoRatio,manualRatio);

    writePathMovingInfo();



    #ifdef D_USE_ROS
    g_rosApi->shutDownRosModule();
    sleep(1);
    #endif
    return 1;
}

int McControllerInterpreter::writePathMovingInfo()
{
    if(false==isInitialOk)
    {
        return -1;
    }
#ifdef D_USE_ROS

    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    QString tmpIniFile=tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/runConfig.ini";
    //
    PathMovingInfo pathinfoOut;
    trajServer->getPathMovingInfo_virtualLineControl(pathinfoOut);
    PositionInfo positionReturn;
    getVehiclePosition_specified(positionReturn,E_VEHICLE_POSITION_TYPE_ROS);
    if(positionReturn.isUpdateSucess)
    {
        robotAttribute->writeLastFloorInfo(tmpIniFile,pathinfoOut,positionReturn.position);
    }
    else
    {
        getVehiclePosition_specified(positionReturn,E_VEHICLE_POSITION_TYPE_CU1);
        if(positionReturn.isUpdateSucess)
        {
            robotAttribute->writeLastFloorInfo(tmpIniFile,pathinfoOut,positionReturn.position);
        }
        else
        {
//            qDebug()<<"error,,,writePathMovingInfo.posotion is failed,amcl position failed,reflect position failed";
            return -1;
        }
    }
#endif
    return 1;

}

int McControllerInterpreter::getWheelData(vector<double> &wheelMoveSpeed, vector<double> &wheelPathLength)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getWheelData(wheelMoveSpeed,wheelPathLength);
}

int McControllerInterpreter::getOdometer(VehicleVelocity &vehicleVelFeedbackOut,
                                         VehiclePosition &vehiclePositionFeedbackOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getOdometer(vehicleVelFeedbackOut,vehiclePositionFeedbackOut);
}

int McControllerInterpreter::vehicleJog(double xMove, double yMove, double rotate)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    if(ENUM_CONTROL_T1 != controlType && ENUM_CONTROL_T2 != controlType)
    {
        qDebug()<<"McControllerInterpreter::vehicleJog current ,不允许手动　controlType="<<controlType;
        addMsg( ENUM_MSG_WARNING, "McControllerInterpreter", "McControllerInterpreter", 5308, robotId );
        return -1;
    }


//    if( (0.000001>fabs(xMove)) )
//    {
//         qDebug()<<"1";
//    }
//    if(  (0.000001>fabs(yMove)) )
//    {
//         qDebug()<<"2";
//    }
//    if( (fabs(rotate)<0.000001) )
//    {
//         qDebug()<<"3";
//    }
//    if( (0.000001>fabs(xMove)) && (0.000001>fabs(yMove)) && (0.000001>fabs(rotate)) )
//    {
//        return -1;
//    }
    trajServer->setMotionServerModel(ENUM_MOTION_MODEL_VEHICLE_JOG);
    PathMovingInfo pathinfoOut;
    trajServer->getPathMovingInfo_virtualLineControl(pathinfoOut);
    pathinfoOut.currentStationId=-1;
    pathinfoOut.currentPathId=-1;
    trajServer->setPathMovingInfo_virtualLineControl(pathinfoOut);
    trajServer->startVehicleJogMove();
    return trajServer->vehicleJog(xMove,yMove,rotate);
}

int McControllerInterpreter::vehicleJogStop()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    if(ENUM_CONTROL_T1 != controlType && ENUM_CONTROL_T2 != controlType)
    {
        qDebug()<<"McControllerInterpreter::vehicleJogStop current ,不允许手动　controlType="<<controlType;
//        addMsg( ENUM_MSG_WARNING, "McControllerInterpreter", "McControllerInterpreter", 5308, robotId );
        return -1;
    }
    return trajServer->vehicleJog(0,0,0);
}

int McControllerInterpreter::vehicleAuto(double xMove, double yMove, double rotate)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->startVehicleJogMove();
    return trajServer->vehicleAuto(xMove,yMove,rotate);
}

//int McControllerInterpreter::vehicleJog(double xMove,  double yMove,double rotate)
//{
//    if(false==isInitialOk)
//    {
//        return -1;
//    }
//    return trajServer->vehicleJog(xMove,yMove,rotate);
//}

int McControllerInterpreter::startVehicleJogMove()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->startVehicleJogMove();
}

int McControllerInterpreter::stopVehicleJogMove()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->stopVehicleJogMove();
}

int McControllerInterpreter::setVehicleMagneticMovePid(int idOfPid,TrackPidInfo pidInfo)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setVehicleMagneticMovePid(idOfPid,pidInfo);
}

int McControllerInterpreter::getJointCommand(QVector<double> &jointsPositionOut, QVector<double> &jointsVelOut, QVector<double> &jointsAccOut, QVector<double> &jointsTorqueOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getJointCommand(jointsPositionOut,jointsVelOut,jointsAccOut,jointsTorqueOut);
}

//int McControllerInterpreter::getMaganeticDi(QVector<bool> &diOut)
//{
//    if(false==isInitialOk)
//    {
//        return -1;
//    }
//    return trajServer->getMaganeticDi(diOut);

//}

int McControllerInterpreter::resetVehicleMagneticMove()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->resetVehicleMagneticMove();

}

int McControllerInterpreter::startVehicleMagneticMove( )
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->startVehicleMagneticMove();
}

int McControllerInterpreter::setMageneticPassRfid(long long passRfidIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setMageneticPassRfid(passRfidIn);
}

int McControllerInterpreter::getTrackStatus(E_TRACK_STATUS &statusOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getTrackStatus(statusOut);
}

int McControllerInterpreter::setTrackMode(E_TRACK_MODE modeIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setTrackMode(modeIn);
}

int McControllerInterpreter::setTrackStopCondition(E_TRACK_STOP_CONDITION conditionIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setTrackStopCondition(conditionIn);
}

int McControllerInterpreter::setVehicleIdealSpeed_ratio(double speedIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setVehicleIdealSpeed_ratio(speedIn*autoRatio);
}

int McControllerInterpreter::setVehicleIdealSpeed_mPerS(double speedIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setVehicleIdealSpeed_mPerS(speedIn*autoRatio);
}

int McControllerInterpreter::readVersion(std::vector <ComponentVersion> &componentVersionList)
{

    componentVersionList.push_back(versionInfo);
    return 1;
}

int McControllerInterpreter::servoOn(int axisId)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->servoOn(axisId);
    return 1;
}


int McControllerInterpreter::servoOn()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->servoOn();
    return 1;
}

int McControllerInterpreter::servoOff(int axisId)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->servoOff(axisId);
    return 1;
}

int McControllerInterpreter::servoOff()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->servoOff_all();
    return 1;
}

bool McControllerInterpreter::isServoOn(int axisId)
{
    if(false==isInitialOk)
    {
        return false;
    }
    return trajServer->isServoOn(axisId);
}

bool McControllerInterpreter::isServoOn()
{
    if(false==isInitialOk)
    {
        return false;
    }
    return trajServer->isServoOn();

}

int McControllerInterpreter::moveOneStepByVelocity(int axisId, double velocityIn)
{
    if(false==isInitialOk)
    {
        return false;
    }
    return trajServer->moveOneStepByVelocity(axisId,velocityIn);
}
int McControllerInterpreter::moveOneStepByVelocity(QVector<double> velocity)
{
    if(false==isInitialOk)
    {
        return false;
    }
    return trajServer->moveOneStepByVelocity(velocity);
}

int McControllerInterpreter::moveOneStepByPV(double positionIn, double velIn ,int axisId)
{
    if(false==isInitialOk)
    {
        return false;
    }
    return trajServer->moveOneStepByPV(positionIn,velIn,axisId);
}

int McControllerInterpreter::setMotorControlMode(int axisId, E_MOTOR_CONTROL_MODE modeIn)
{
    if(false==isInitialOk)
    {
        return false;
    }
    return trajServer->setMotorControlMode(axisId,modeIn);
}

//int McControllerInterpreter::addMcInterpreterRegData(RegData *regData)
//{
//    mcInterpreterRegData = regData;
//    return 1;
//}

int McControllerInterpreter::updateControllerInfoLevel2( UpdateDataLv2  &updateDataLv2 )
{
    if(false==isInitialOk)
    {
        return -1;
    }


        updateDataLv2.globalProSpeed = (float)(100 * autoRatio);
        updateDataLv2.globalManSpeed = (float)(100 * manualRatio);
        updateDataLv2.globalCoordinate = jogControl->getJogCoordinate();
        updateDataLv2.globalDefaultUserNum =  getUserIndex();
        updateDataLv2.globalDefaultToolNum = getToolIndex();
        updateDataLv2.globalCurrentState_motion=trajServer->getStatus();
        //
        trajServer->getCurrentJointPoint_Feedback(updateDataLv2.currentPositionJoint);
        trajServer->getCurrentCoordinatePoint_Feedback(updateDataLv2.currentPositionWorld);
        E_MOVING_FLAG tmpMFlag;
        trajServer->getArmMovingFlag(updateDataLv2.armMovingLineId,tmpMFlag);
        updateDataLv2.armMovingFlag=tmpMFlag;


    return 1;
}




///**
// * @brief McControllerInterpreter::getZeroMotorPosition, 返回各个关节电机编码值
// * @return
// */
//QVector<int> McControllerInterpreter::getZeroMotorPosition()
//{
//    if(false==isInitialOk)
//    {
//        QVector<int> tmpReturn;
//        tmpReturn.resize(1000);
//        return tmpReturn;
//    }
//    QVector<int> value;
//    trajServer->getCalibrateMotorValueOffset(value);
//    return value;
//}

int McControllerInterpreter::getTrajServerStatus()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getStatus();
}

int McControllerInterpreter::getStatus()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    status=trajServer->getStatus();
    return status;
}

double McControllerInterpreter::getAutoRatio()
{
    return autoRatio;
}

int McControllerInterpreter::getRunMode()
{
    return runMode;
}

int McControllerInterpreter::getMoveType()
{
    return moveType;
}

int McControllerInterpreter::switchMoveType(int moveTypeIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    if((ENUM_MOVE_ORDER_STEP==moveTypeIn)||(ENUM_MOVE_HAND_SERIES==moveTypeIn))
    {
        moveType = moveTypeIn;
        if(ENUM_MOVE_ORDER_STEP==moveTypeIn)
        {
            trajServer->setTrajectoryContinuousRun(false);
        }
        else
        {
            trajServer->setTrajectoryContinuousRun(true);
        }
    }
    else
    {
        moveType = ENUM_MOVE_END;
    }
    return 1;
}

int McControllerInterpreter::getToolIndex()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getToolIndex();
}

int McControllerInterpreter::getUserIndex()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getUserIndex();
}

int McControllerInterpreter::getJointNum()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return jointNum;
}

float McControllerInterpreter::getJointMaxPosition(int jointIndex)
{
    if(false==isInitialOk)
    {
        return -1;
    }

    return trajServer->getJointMaxPosition(jointIndex);

}

float McControllerInterpreter::getJointMinPosition(int jointIndex)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getJointMinPosition(jointIndex);

}

int McControllerInterpreter::getJointType(int jointIndex)
{
    if(false==isInitialOk)
    {
        return -1;
    }

    return trajServer->getJointType(jointIndex);

}

int McControllerInterpreter::getAxisInformation(std::vector<AxisLimit> &axisLimit,
                                                std::vector<AxisLimit> &extAxisLimit,
                                                std::vector<int> &axisUnit, std::vector<int> &extAxisUnit,
                                                std::vector<int> &axisType, std::vector<int> &extAxisType)
{
    if(false==isInitialOk)
    {
        return -1;
    }

    return trajServer->getAxisInformation(axisLimit,extAxisLimit,axisUnit,extAxisUnit,axisType,extAxisType);
}


int McControllerInterpreter::getJogCoordinate()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return jogControl->getJogCoordinate();
}

int McControllerInterpreter::addTrajectoryCommand(QVector<MoveParameter> tmpCommandVector)
{
    if(false==isInitialOk)
    {
        return -1;
    }

    return trajServer->addTrajectoryCommand(tmpCommandVector); 
}

int McControllerInterpreter::transCommandQueueToCarvingModule(QVector<MoveParameter> &tmpCommandVectorIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->transCommandQueueToCarvingModule(tmpCommandVectorIn);
}

int McControllerInterpreter::startOneModel(MC_MOTION_MODEL modelIn)
{
    qDebug()<<"McControllerInterpreter::startOneModel!";
    if(false==isInitialOk)
    {
        return -1;
    }

    if(ENUM_INTEPRETER_INIT_ERROR ==status)
    {
        return 0;
    }


    trajServer->startOneModel(modelIn);
}

int McControllerInterpreter::allStartRun()
{
    qDebug()<<"McControllerInterpreter::startMove!";
    if(false==isInitialOk)
    {
        return -1;
    }

    if(ENUM_INTEPRETER_INIT_ERROR ==status)
    {
        return 0;
    }

    if(ENUM_CONTROL_AUTO <= controlType)//自动或者外部模式
    {
        realRatio = autoRatio;
    }
    else if(ENUM_CONTROL_T2 == controlType)//T2
    {
        realRatio = manualRatio;
    }
    else//T1
    {
        realRatio = manualRatio * t1RatioLimit;
    }

    status = trajServer->getStatus();
    trajServer->setMianControlRatio(1);
    if(status!=ENUM_TRAJECTORY_CEASED && status!=ENUM_TRAJECTORY_CEASING)
    {
        qDebug()<<"trajServer status"<<status;
        trajServer->resetAll_and_virtualLine();
        trajServer->setControlSpeedRatio(realRatio,3);
    }
    else
    {
//        trajServer->setControlSpeedRatio(realRatio,0.2);//0.2s对于agv高速度下恢复太快了。
        trajServer->setControlSpeedRatio(realRatio,3);//0.2s对于agv高速度下恢复太快了。
    }

//    trajServer->trajectoryRun();
    trajServer->allStartRun();


    return 1;
}

int McControllerInterpreter::allStartRun_fleet()
{
    qDebug()<<"McControllerInterpreter::allStartRun_fleet!";
    if(false==isInitialOk)
    {
        return -1;
    }

    if(ENUM_INTEPRETER_INIT_ERROR ==status)
    {
        return 0;
    }

    if(ENUM_CONTROL_AUTO <= controlType)//自动或者外部模式
    {
        realRatio = autoRatio;
    }
    else if(ENUM_CONTROL_T2 == controlType)//T2
    {
        realRatio = manualRatio;
    }
    else//T1
    {
        realRatio = manualRatio * t1RatioLimit;
    }

    status = trajServer->getStatus();
    trajServer->setMianControlRatio(1);
    if(status!=ENUM_TRAJECTORY_CEASED)
    {
        trajServer->resetAll_and_virtualLine_no_clear_queue();
        trajServer->setControlSpeedRatio(realRatio,3);
    }
    else
    {
//        trajServer->setControlSpeedRatio(realRatio,0.2);//0.2s对于agv高速度下恢复太快了。
        trajServer->setControlSpeedRatio(realRatio,3);//0.2s对于agv高速度下恢复太快了。
    }

//    trajServer->trajectoryRun();
    trajServer->allStartRun();


    return 1;
}

int McControllerInterpreter::allCease()
{
    qDebug()<<"McControllerInterpreter::cease!";
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->allCease();
    status=trajServer->getStatus();
    //writeDataToFile();

    return 1;
}

int McControllerInterpreter::allStop()
{
    if(false==isInitialOk)
    {
        return -1;
    }
//hualei todo
    //cease();
    trajServer->allStop();

    //quickStop();

//    trajServer->stop();
//    status=trajServer->getStatus();
//    writeDataToFile();

    return 1;
}



int McControllerInterpreter::quickStop()
{
    //不需要担心报错后，认为挪动了一段距离，如果沿着旧轨迹运行，会有跟随误差和速度偏差报错。
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->quickStop();
    stopMotorWave();
    status=trajServer->getStatus();
    if(1==debugFlag)
    {
//        qDebug()<<"McControllerInterpreter::quickStop====清空指令缓存" << status;
    }

    return 1;
}

int McControllerInterpreter::resetAll()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->resetAll();
    status=trajServer->getStatus();
    return 1;
}


int McControllerInterpreter::initialServoParameter()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    #ifndef MOTION_SERVER_UNIT_TEST
   trajServer->initialServoParameter();

    #endif
    return 1;
}

int McControllerInterpreter::setCheckMode(E_CHECK_MODE checkModeIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setCheckMode(checkModeIn);

}

int McControllerInterpreter::jog(int axis, int direction,int velocityOpt)
{
    if(false==isInitialOk)
    {
        return -1;
    }

//    if (0 == isServoParameterInitialed)
//    {
//        initialServoParameter();
//        isServoParameterInitialed = 1;
//    }

    if((ENUM_CONTROL_T1 != controlType))
    {
        if(debugFlag)
        {
            qDebug() << "McControllerInterpreter::jog====寸动失败，控制模式不是T1"<<controlType<<status;
        }
        addMsg(ENUM_MSG_REMIND,"McControllerInterpreter","McControllerInterpreter", 5303, robotId);
        return 0;
    }
    if( ENUM_CONTROL_T1==controlType )
    {
        trajServer->setCheckMode(E_CHECK_MODE_T2);//坐标运动容易超过Ｔ１
    }
    int trajStatus=trajServer->getStatus();
    if(ENUM_TRAJECTORY_RUN==trajStatus)
    {
        if(debugFlag)
        {
            qDebug() << "McControllerInterpreter::jog====寸动失败，当前为运行状态"<<controlType<<status;
        }
        addMsg(ENUM_MSG_REMIND,"McControllerInterpreter","McControllerInterpreter", 5322, robotId);
        return 0;
    }

//    if(ENUM_TRAJECTORY_CEASED!=trajStatus && ENUM_TRAJECTORY_STOPPED!=trajStatus)
//    {
//        if(debugFlag)
//        {
//            qDebug() << "McControllerInterpreter::jog====寸动失败，但前非停止或暂停状态"<<controlType<<status;
//        }
//        addMsg(ENUM_MSG_REMIND,"McControllerInterpreter","McControllerInterpreter", 5304, robotId);
//        return 0;
//    }

    if(1)
    {
        qDebug() << "McControllerInterpreter::jog====" << axis << direction << velocityOpt;
    }

    trajServer->resetAll();
    trajServer->setMianControlRatio(1);
    realRatio = manualRatio * t1RatioLimit;

    trajServer->setControlSpeedRatio(realRatio,D_JOG_START_TIME);

    jogControl->jog(axis,direction,velocityOpt);

    trajServer->trajectoryRun();
    trajServer->setBasicStatus(ENUM_TRAJECTORY_RUN);


    return 1;
}

int McControllerInterpreter::setJogCoordinate(int coordinateType)
{
    if(false==isInitialOk)
    {
        return -1;
    }

    jogControl->setJogCoordinate(coordinateType);
    if(debugFlag)
    {
        qDebug()<< "McControllerInterpreter::setJogCoordinate====切换寸动坐标";
    }
    return 1;
}

int McControllerInterpreter::activateToolCoordinate(int index)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->activateToolCoordinate(index);
    return 1;
}

int McControllerInterpreter::activateUserCoordinate(int index)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->activateUserCoordinate(index);
    return 1;
}

int McControllerInterpreter::setToolCoordinate(ToolCoordinateInfo& toolCoordinate)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->setToolCoordinate(toolCoordinate);
    return 1;
}

int McControllerInterpreter::setUserCoordinate(UserCoordinateInfo& userCoordinate)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->setUserCoordinate(userCoordinate);
    return 1;
}


int McControllerInterpreter::algorithmFunctioncall(const QVector<bool> &servoOnStatus, const QVector<MotorData> &motorFeedback,
                                                   QVector<MotorData> &motorCommandOut, int &returnMutexFlag)
{
    motorCommandOut.clear();
    //test time
//    returnMutexFlag=-1;//不能上使能，否则电机突然乱跑。
//    return -1;

    if(false==isInitialOk)
    {
        returnMutexFlag=-1;
        return -1;
    }
    QVector<E_MOTOR_CONTROL_MODE> controlModeOut;
    if(1==isUsePositionRegulatorStatus)
    {
        positionRegulatorLoop(servoOnStatus,motorFeedback,motorCommandOut);
        returnMutexFlag=0;
        return 1;

    }
    else if(2==isUsePositionRegulatorStatus)
    {
        positionAndVelocityRegulatorLoop(servoOnStatus,motorFeedback,motorCommandOut);
        returnMutexFlag=0;
        return 1;

    }
    else
    {
        return trajServer->algorithmFunctioncall(servoOnStatus,motorFeedback,motorCommandOut,returnMutexFlag,controlModeOut);
    }

    return 0;
}

//需要添加判断是否要初始化的条件，以免原有的码垛数据被新数据覆盖
int McControllerInterpreter::palletInit()//码垛初始化
{
    if(false==isInitialOk)
    {
        return -1;
    }

    return 1;
}

//2017-3-30 添加修改
int McControllerInterpreter::calculateInitPallet()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return 1;
}

// 2017-4-21 添加
int McControllerInterpreter::setPalletPlFlag(int palletIndex)
{
    if(false==isInitialOk)
    {
        return -1;
    }

    return 1;
}

int McControllerInterpreter::setPalletFirstFlag(int palletIndex)
{
    if(false==isInitialOk)
    {
        return -1;
    }
   return 1;
}

int McControllerInterpreter::calculateNextPallet(int palletNumber)
{
    if(false==isInitialOk)
    {
        return -1;
    }

    return 1;
}



//PalletSave McControllerInterpreter::getPalletVector(int palletIndex)
//{
//    PalletSave ps;
//    return ps;
//}

int McControllerInterpreter::setPl(int index, int row, int column,  int layer)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return 1;
}

int McControllerInterpreter::setRollerSpeed(float _rollerRatio)
{
    if(false==isInitialOk)
    {
        return -1;
    }

    if(isRollerFlag)//使用滚轮,手动模式,运行状态
    {
        trajServer->setMianControlRatio(1);
        if( ENUM_CONTROL_T2==controlType )//T2模式
        {
            realRatio = rollerRatio;

            if(debugFlag)
            {

            }
        }
        else if( ENUM_CONTROL_T1==controlType )//T1模式
        {
            realRatio = rollerRatio * t1RatioLimit;

            if(debugFlag)
            {

            }
        }
        if(ENUM_CONTROL_T2==controlType || ENUM_CONTROL_T1==controlType)
        {
            trajServer->setControlSpeedRatio(realRatio,3);
        }

    }
    if(debugFlag)
    {
        qDebug()<< "McControllerInterpreter::setRollerSpeed====end"<< rollerRatio << realRatio << status <<controlType<<isRollerFlag;
    }

    return 1;
}

int McControllerInterpreter::setManulSpeed(float _manualRatio)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    manualRatio = _manualRatio / 100.0;
    if(manualRatio>1)
    {
        qDebug()<< "manualRatio too big ="<<manualRatio;
        manualRatio=1;
    }
    if(!isRollerFlag)//非滚轮,运行状态
    {
        if( ENUM_CONTROL_T2==controlType )//T2模式
        {
            realRatio = manualRatio;           
            if(debugFlag)
            {

            }
        }
        else if( ENUM_CONTROL_T1==controlType )//T1模式
        {
            realRatio = manualRatio * t1RatioLimit;
            if(debugFlag)
            {

            }
        }
        if(ENUM_CONTROL_T2==controlType || ENUM_CONTROL_T1==controlType)
        {
            trajServer->setControlSpeedRatio(realRatio,2);
        }


    }
    if(debugFlag)
    {
        qDebug()<< "McControllerInterpreter::setManulSpeed====end"
                <<"manualRatio"<<manualRatio<<"realRatio"<<realRatio << "status"<< status
               <<"controlType"<<controlType<<"isRollerFlag"<<isRollerFlag;
    }

    return 1;
}

int McControllerInterpreter::setAutoSpeed(float ratioIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    autoRatio = ratioIn / 100.0;

    trajServer->setMaganeticMovingSpeedRatio(autoRatio);

    if(autoRatio>1)
    {
        qDebug()<< "autoRatio too big ="<<autoRatio;
        autoRatio=1;
    }
    if(ENUM_CONTROL_AUTO<=controlType)//自动,运行状态
    {
        realRatio = autoRatio;
        trajServer->setControlSpeedRatio(realRatio,2);
        if(debugFlag)
        {

        }
    }
    if(debugFlag)
    {
        qDebug()<< "McControllerInterpreter::setAutoSpeed====end"<<autoRatio<<realRatio<<status<<controlType
                <<"ratioIn"<<ratioIn;
    }
    return 1;

}

PointPro McControllerInterpreter::pointCoordinateToJoint( PointPro pointIn)
{
//    trajServer->printPointPro(pointIn,"pointIn pointCoordinateToJoint");
//    return pointIn;

    PointPro tmpPoint;
    tmpPoint=pointIn;
    if(false==isInitialOk)
    { 
        qDebug()<<"McControllerInterpreter::pointCoordinateToJoint failed! false==isInitialOk";

        return tmpPoint;
    }


    if(trajServer->getStatus()==ENUM_TRAJECTORY_STOPPED || trajServer->getStatus()==ENUM_TRAJECTORY_CEASED)
    {
        trajServer->pointCoordinateToJoint_noDeviationCheck(pointIn,tmpPoint);
        trajServer->printPointPro(tmpPoint,"tmpPoint2 pointCoordinateToJoint");
        return tmpPoint;
    }
    else
    {
        qDebug()<<"pointCoordinateToCartesian failed! trajServer->getStatus()==ENUM_TRAJECTORY_STOPPED || trajServer->getStatus()==ENUM_TRAJECTORY_CEASED";
        return tmpPoint;
    }
    return tmpPoint;
}

PointPro McControllerInterpreter::pointCoordinateToCartesian(PointPro pointIn)
{
//    trajServer->printPointPro(pointIn,"pointIn pointCoordinateToCartesian");
//    return pointIn;

    PointPro tmpPoint;
    tmpPoint=pointIn;
    if(false==isInitialOk)
    {
        qDebug()<<"McControllerInterpreter::pointCoordinateToCartesian failed! false==isInitialOk";

        return tmpPoint;
    }


//    if(trajServer->getStatus()==ENUM_TRAJECTORY_STOPPED || trajServer->getStatus()==ENUM_TRAJECTORY_CEASED)
//    {

        if(1!=trajServer->pointCoordinateToCartesian(tmpPoint,tmpPoint))
        {
            qDebug()<<"----------error jjgsddd";
//            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5059,robotId);
            return tmpPoint;
        }
        trajServer->printPointPro(tmpPoint,"tmpPoint2 pointCoordinateToCartesian");
        return tmpPoint;
//    }
//    else
//    {
//        qDebug()<<"pointCoordinateToCartesian failed! trajServer->getStatus()==ENUM_TRAJECTORY_STOPPED || trajServer->getStatus()==ENUM_TRAJECTORY_CEASED";
//        return tmpPoint;
//    }

    return tmpPoint;

}

int McControllerInterpreter::returnZeroPoint(int axisNumber, int &returnFlag)
{
    if(false==isInitialOk)
    {
        return -1;
    }

    if(-1==axisNumber)
    {
        if(true==trajServer->isAxisZeroOk())
        {
            int returnFlag;
            return moveToZeroPoint(axisNumber,returnFlag);
        }
        else
        {
            return trajServer->startGoZero_forUser();
        }
    }
    else  if(0<=axisNumber)
    {
        if(true==trajServer->isAxisZeroOk(axisNumber))
        {
            int returnFlag;
            return moveToZeroPoint(axisNumber,returnFlag);
        }
        else
        {
            return trajServer->startGoZero_forUser(axisNumber);
        }
    }
    return -1;

}
int McControllerInterpreter::moveToZeroPoint(int axisNumber, int &returnFlag)
{
    if(false==isInitialOk)
    {
        return -1;
    }

    if( ENUM_CONTROL_T1==controlType )
    {
        trajServer->setCheckMode(E_CHECK_MODE_T1);
    }

    if(ENUM_CONTROL_T1!=controlType)
    {
        return -1;
    }

    trajServer->setMianControlRatio(1);
    realRatio = manualRatio * t1RatioLimit;
    trajServer->setControlSpeedRatio(realRatio,0.2);
    return jogControl->returnZeroPoint(axisNumber,returnFlag);

}

int McControllerInterpreter::moveToPoint(const MoveParameter target)//移动到点没有滚轮调速的功能
{
    if(false==isInitialOk)
    {
        return -1;
    }

    if( ENUM_CONTROL_T1==controlType )
    {
        trajServer->setCheckMode(E_CHECK_MODE_T2);//坐标运动容易超过Ｔ１
    }

    if(ENUM_CONTROL_T1!=controlType)
    {
        return -1;
    }
    trajServer->setMianControlRatio(1);
    realRatio = manualRatio * t1RatioLimit;
    trajServer->setControlSpeedRatio(realRatio,0.2);

    return jogControl->moveToPoint(target);

}

int McControllerInterpreter::getSoftLimit(std::vector<std::vector<float> > &axisLimit, std::vector<int> &axisUnit)
{
    if(false==isInitialOk)
    {
        axisLimit.clear();
        axisUnit.clear();
        return -1;
    }
    trajServer->getSoftLimit(axisLimit,axisUnit);
    return 1;
}


int McControllerInterpreter::trajectoryProcess()
{

    return 1;

}




int McControllerInterpreter::updateCurrentPoint_FeedBack()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    PointPro currentJoint;
    PointPro currentCoordinate;
    trajServer->getCurrentJointPoint_Feedback(currentJoint);
    trajServer->getCurrentCoordinatePoint_Feedback(currentCoordinate);
    return 1;

}


int McControllerInterpreter::getCurrentJointPoint_Feedback(PointPro &jointPoint)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->getCurrentJointPoint_Feedback(jointPoint);
    return 1;
}

int McControllerInterpreter::getJoint_Feedback(QVector<double> &jointsPosition, QVector<double> &jointsVel, QVector<double> &jointAcc, QVector<double> &jointTorque)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->getFeedbackJoint(jointsPosition, jointsVel, jointAcc,jointTorque);
    return 1;
}


int McControllerInterpreter::getCurrentCoordinatePoint_Feedback(PointPro &coordinatePoint)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->getCurrentCoordinatePoint_Feedback(coordinatePoint);
    return 1;

}
\
float McControllerInterpreter::getRunningNumber()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getRunningNumber();
}

double McControllerInterpreter::getBarrierProtectRatioOfMoving()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getBarrierProtectRatioOfMoving();
}

float McControllerInterpreter::getRunningNumber_virtualLineControl()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getRunningNumber_virtualLineControl();
}

int McControllerInterpreter::setRunningNumber_virtualLineControl(float numberIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setRunningNumber_virtualLineControl(numberIn);
}

int McControllerInterpreter::setRunningNumber_robotArmControl(float numberIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setRunningNumber_robotArmControl(numberIn);
}

int McControllerInterpreter::setPathMovingInfo_virtualLineControl(PathMovingInfo pathInfoIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->setPathMovingInfo_virtualLineControl(pathInfoIn);
}

int McControllerInterpreter::getPathMovingInfo_virtualLineControl(PathMovingInfo &infoOut)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    return trajServer->getPathMovingInfo_virtualLineControl(infoOut);
}

//VREP 用了
int McControllerInterpreter::getVrepRadianToEncoder( int axis,  float &radianVec )
{
    if(false==isInitialOk)
    {
        return -1;
    }
    int tmp=-1;
    if(ENUM_INTEPRETER_INIT_ERROR==status)
    {
        return tmp;
    }
    if(axis>=robotParameter.jointAttributeVector.size())
    {
        qDebug()<<"error,getVrepRadianToEncoder";
        return 0;
    }

    QVector<double> tmpJointsPosition,tmpJointsVel,tmpJointAcc, tmpJointTorque;
    tmpJointsPosition.resize(jointNum);
    tmpJointsVel.resize(jointNum);
    tmpJointAcc.resize(jointNum);
    tmpJointTorque.resize(jointNum);
    if(0==robotParameter.jointAttributeVector[axis].jointType)
    {
        tmpJointsPosition[axis]=180*radianVec/M_PI;
    }
    else
    {
        tmpJointsPosition[axis]=radianVec*1000;//vrep 采用ＲＡＤ、Ｍ　UNITS
    }
    jointDecouple->setDecoupleData( tmpJointsPosition, tmpJointsVel, tmpJointAcc, tmpJointTorque);
    if(-1==jointDecouple->executeDecoupleToPulse() )
    {
        return -1;
    }

    QVector<MotorData> returnMotorData=jointDecouple->getMotorDataCommandVector();
    tmp=returnMotorData[axis].position;
    return tmp;
}

//VREP 用了
float McControllerInterpreter::getVrepEncoderToRadian( int axis,  const int &encoderVec )
{
    if(false==isInitialOk)
    {
        return -1;
    }

    float tmp=-1;
    if(ENUM_INTEPRETER_INIT_ERROR==status)
    {
        return tmp;
    }

    if(axis>=robotParameter.jointAttributeVector.size())
    {
        qDebug()<<"error,getVrepEncoderToRadian";
        return 0;
    }


    QVector<MotorData> motorFeedback;
    motorFeedback.resize(jointNum);
    motorFeedback[axis].position=encoderVec;
    jointDecouple->setMotorFeedbackDataVector(motorFeedback);

    QVector<double> tmpJointsPosition,tmpJointsVel,tmpJointAcc, tmpJointTorque;
    tmpJointsPosition.resize(jointNum);
    tmpJointsVel.resize(jointNum);
    tmpJointAcc.resize(jointNum);
    tmpJointTorque.resize(jointNum);

    jointDecouple->getFeedbackJoint(tmpJointsPosition, tmpJointsVel, tmpJointAcc, tmpJointTorque);
    if(0==robotParameter.jointAttributeVector[axis].jointType)
    {
        tmp=M_PI*tmpJointsPosition[axis]/180;
    }
    else
    {
        tmp=tmpJointsPosition[axis]/1000;//vrep 采用ＲＡＤ、Ｍ　UNITS
    }


    return tmp;
}




//编码器值转换为角度360. PowerManager用了
QVector<float> McControllerInterpreter::getEncoderToAngle(const QVector<int> &encoderVec )
{
    if(false==isInitialOk)
    {
        QVector<float> tmpReturn;
        tmpReturn.resize(100);
        return tmpReturn;
    }
    QVector<float> angleVec;
    if(ENUM_INTEPRETER_INIT_ERROR==status){
        return angleVec;
    }
    angleVec.resize(jointNum);

    QVector<MotorData> motorFeedback;
    motorFeedback.resize(jointNum);
    for(int i=0;i<jointNum;i++)
    {
        motorFeedback[i].position=encoderVec[i];
    }
    jointDecouple->setMotorFeedbackDataVector(motorFeedback);

    QVector<double> tmpJointsPosition,tmpJointsVel,tmpJointAcc, tmpJointTorque;
    tmpJointsPosition.resize(jointNum);
    tmpJointsVel.resize(jointNum);
    tmpJointAcc.resize(jointNum);
    tmpJointTorque.resize(jointNum);
    jointDecouple->getFeedbackJoint(tmpJointsPosition, tmpJointsVel, tmpJointAcc, tmpJointTorque);

    for(int i=0;i<jointNum;i++)
    {
        angleVec[i]=tmpJointsPosition[i];
    }

    return angleVec;
}




int McControllerInterpreter::setMotionServerModel(MC_MOTION_MODEL modelIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->setMotionServerModel(modelIn);

    return 1;
}


int McControllerInterpreter::controlTypeSwitch(int controlTypeIn)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->controlTypeSwitch(controlTypeIn);
    updateStatus();
    if((controlType == controlTypeIn)||(ENUM_INTEPRETER_INIT_ERROR==status)){
        return -1;
    }
    int trajStatus=trajServer->getStatus();
    if ( trajStatus == ENUM_TRAJECTORY_CEASED || trajStatus == ENUM_TRAJECTORY_STOPPED )//非运行，可以切换
    {
        controlType = controlTypeIn;

        if(ENUM_CONTROL_AUTO==controlType)
        {
            realRatio = autoRatio;
            trajServer->setCheckMode(E_CHECK_MODE_T2);
        }
        else if(ENUM_CONTROL_FLEET==controlType)
        {
            realRatio = autoRatio;
            trajServer->setCheckMode(E_CHECK_MODE_T2);
        }
        else if( ENUM_CONTROL_T2==controlType )//切换为T2
        {
            if(isRollerFlag){//滚轮
                realRatio = rollerRatio;
            }
            else{
                realRatio = manualRatio;
            }
            trajServer->setCheckMode(E_CHECK_MODE_T2);
        }
        else if( ENUM_CONTROL_T1==controlType )//切换为T1
        {
            if(isRollerFlag){//滚轮
                realRatio = rollerRatio* t1RatioLimit;
            }
            else{
                realRatio = manualRatio* t1RatioLimit;
            }
            trajServer->setCheckMode(E_CHECK_MODE_T1);
        }
        trajServer->setControlSpeedRatio(realRatio,3);


        return 1;
    }
    else
    {
        addMsg(ENUM_MSG_REMIND,"McControllerInterpreter","McControllerInterpreter", 5301, robotId);
        return -1;
    }
}



QVector<MotorData> McControllerInterpreter::getMotorFeedbackDataVector()
{
    if(false==isInitialOk)
    {
        QVector<MotorData> tmpReturn;
        tmpReturn.resize(100);
        return tmpReturn;
    }
    return trajServer->getMotorFeedbackDataVector();
}


int McControllerInterpreter::writeDataToFile()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    trajServer->writeDataToFile();
    return 1;
}

int McControllerInterpreter::isTrajectoryEmpty()
{
    if(false==isInitialOk)
    {
        return -1;
    }

    return trajServer->isTrajectoryEmpty();
}
PingPongBuffer* McControllerInterpreter::getPingPongBuffer()
{
    if(false==isInitialOk)
    {
        return NULL;
    }

    return trajServer->getPingPongBuffer();
}

QVector<ToolCoordinateInfo> McControllerInterpreter::getToolCoordinateList()
{
    if(false==isInitialOk)
    {
        QVector<ToolCoordinateInfo> tmpReturn;
        tmpReturn.resize(COORDINATE_MAX_SIZE);
        return tmpReturn;
    }
    return trajServer->getToolCoordinateList();
}

QVector<UserCoordinateInfo> McControllerInterpreter::getUserCoordinateList()
{
    if(false==isInitialOk)
    {
        QVector<UserCoordinateInfo> tmpReturn;
        tmpReturn.resize(COORDINATE_MAX_SIZE);
        return tmpReturn;
    }
    return trajServer->getUserCoordinateList();
}

int McControllerInterpreter::updateStatus()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    status=trajServer->getStatus();
    return 1;
}




int McControllerInterpreter::gravityTest()
{
    if(false==isInitialOk)
    {
        return -1;
    }
//    QVector <double> thresholdValue;
//    QVector <double> proportionValue;
//    QVector <int> integralValue;
//    QVector <int> axisEnable;
//    thresholdValue.resize(jointNum);
//    proportionValue.resize(jointNum);
//    integralValue.resize(jointNum);
//    axisEnable.resize(jointNum);

//    thresholdValue[0] =10;
//    proportionValue[0] = 0.00001;
//    integralValue[0] = 300;
//    thresholdValue[1] = 30;
//    proportionValue[1] = 0.00002;
//    integralValue[1] = 100;
//    thresholdValue[2] = 10;
//    proportionValue[2] = 0.00002;
//    integralValue[2] = 300;
//    thresholdValue[3] = 10;
//    proportionValue[3] = 0.00001;
//    integralValue[3] = 300;
//    thresholdValue[4] = 5;
//    proportionValue[4] = 0.00007;
//    integralValue[4] = 100;
//    thresholdValue[5] = 5;
//    proportionValue[5] = 0.00002;
//    integralValue[5] = 300;
//    if (jointNum == 7)
//    {
//        thresholdValue[6] = 2;
//        proportionValue[6] = 0.00009;
//        integralValue[6] = 100;
//    }


//    for (int i = 0; i < jointNum; i ++)
//    {
//        axisEnable[i] = 1;
//    }

//    if (axisId != -1)
//    {
//        for (int i = 0; i < jointNum; i ++)
//        {
//            axisEnable[i] = 0;
//        }
//        axisEnable[axisId] = 1;
//    }
//    JntArray velocity_idSolver(jointNum);
//    JntArray acc_idSolver(jointNum);
//    Wrenches f_ext;
//    f_ext.resize(jointNum);
//    JntArray gravity(jointNum);
//    for (int i = 0; i < jointNum; i++)
//    {
//        velocity_idSolver(i) = 0;
//        acc_idSolver(i) = 0;
//    }
//    coordinate->getIdSolver_RNE(jointDecouple->positionCommand, velocity_idSolver, acc_idSolver, f_ext, gravity);
//    for (int i = 0; i < jointNum; i++)
//    {
//        calculatedTorques[i] = gravity(i);
//    }

//    JntArray position(jointNum), velocity(jointNum), acc(jointNum), torques(jointNum);
//    jointDecouple->getFeedbackJoint(position, velocity, acc, torques);
//    //jointDecouple->positionCommand=position;

//    for (int i = 0; i < jointNum; i++)
//    {
//        torques(i) = torques(i) * robotJoints[i].maxToeque / 1000.0;
//        calculatedTorques[i] = torques(i);
//    }
//    vector<double> tmpGravity;
//    vector<double> tmpTorques;
//    tmpGravity.resize(2*jointNum);
//    tmpTorques.resize(jointNum);
//    for (int i = 0; i < jointNum; i++)
//    {
//        tmpGravity[i] = gravity(i);
//        tmpGravity[i+jointNum] = torques(i);
//    }

//    QVector<double> torqueOffset, aveTorqueOffset;
//    QQueue<double>  temTorqueOffsetQueue;
//    torqueOffset.resize(jointNum);
//    aveTorqueOffset.resize(jointNum);
//    for (int k = 0; k < jointNum; k++)
//    {
//        if (axisEnable[k] == 1)
//        {
//            torqueOffset[k] = torques(k) - gravity(k);
//            aveTorqueOffset[k] =0;
//            if (averageTorqueOffsetQueue[k].size() > integralValue[k])
//            {
//                averageTorqueOffsetQueue[k].dequeue();
//                temTorqueOffsetQueue = averageTorqueOffsetQueue[k];
//                double sumTorque = 0 ;
//                for (int i = 0; i < averageTorqueOffsetQueue[k].size(); i++)
//                {
//                    sumTorque += temTorqueOffsetQueue.dequeue();
//                }
//                aveTorqueOffset[k] = sumTorque / averageTorqueOffsetQueue[k].size();
//            }
//           averageTorqueOffsetQueue[k].enqueue(torqueOffset[k]);
//            if( fabs(aveTorqueOffset[k] )> thresholdValue[k])
//            {

//                jointDecouple->positionCommand(k) -=proportionValue[k]*aveTorqueOffset[k];
//                //motorDataVector[0].position += 1*aveTorqueOffset;
//            }

//            qDebug()<<"0000000000000000*****************axisId" << k;
//            qDebug()<<"11111111111111111111*************torqueOffset"<<torqueOffset[k]<<
//                      "calculate gravity="<<gravity(k)<<"feedback torque="<<torques(k) << "motorFeedback torque" << motorFeedback[k].torque;
//            qDebug()<<"2222222222222222222*****************aveTorqueOffset"<<aveTorqueOffset[k];
//        }
//    }
//    jointDecouple->executeDecoupleToPulse();
//    analaysizerGravity->recordDataForAnalysize(tmpGravity);
        //analysizerTorques->recordDataForAnalysize(tmpTorques);
    return 1;
}

int McControllerInterpreter::dragLoop(int axisId)
{
    if(false==isInitialOk)
    {
        return -1;
    }
    QVector <double> thresholdValue;
    QVector <double> proportionValue;
    QVector <int> integralValue;
    QVector <int> axisEnable;
    thresholdValue.resize(jointNum);
    proportionValue.resize(jointNum);
    integralValue.resize(jointNum);
    axisEnable.resize(jointNum);

    thresholdValue[0] =10;
    proportionValue[0] = 0.00002;
    integralValue[0] = 300;
    thresholdValue[1] = 35;
    proportionValue[1] = 0.00002;
    integralValue[1] = 100;
    thresholdValue[2] = 10;
    proportionValue[2] = 0.00005;
    integralValue[2] = 300;
    thresholdValue[3] = 15;
    proportionValue[3] = 0.00005;
    integralValue[3] = 300;
    thresholdValue[4] = 5;
    proportionValue[4] = 0.00017;
    integralValue[4] = 100;
    thresholdValue[5] = 5;
    proportionValue[5] = 0.00012;
    integralValue[5] = 300;
    if (jointNum == 7)
    {
        thresholdValue[6] = 2;
        proportionValue[6] = 0.00019;
        integralValue[6] = 100;
    }

    for (int i = 0; i < jointNum; i ++)
    {
        axisEnable[i] = 1;
    }

    if (axisId != -1)
    {
        for (int i = 0; i < jointNum; i ++)
        {
            axisEnable[i] = 0;
        }
        axisEnable[axisId] = 1;
    }

//    JntArray position(jointNum), velocity(jointNum), acc(jointNum), torques(jointNum);
//    jointDecouple->getFeedbackJoint(position, velocity, acc, torques);

//    for (int i = 0; i < jointNum; i++)
//    {
//        torques(i) = torques(i) * robotJoints[i].maxToeque / 1000.0;
//    }
    vector<double> tmpGravity;
    vector<double> tmpTorques;
    tmpGravity.resize(2*jointNum);
    tmpTorques.resize(jointNum);
//    for (int i = 0; i < jointNum; i++)
//    {
//        tmpGravity[i] = calculatedTorques[i];
//        tmpGravity[i+jointNum] = torques(i);
//    }
//    QVector<double> torqueOffset, aveTorqueOffset;
//    QQueue<double>  temTorqueOffsetQueue;
//    torqueOffset.resize(jointNum);
//    aveTorqueOffset.resize(jointNum);
//    for (int k = 0; k < jointNum; k++)
//    {
//        if (axisEnable[k] == 1)
//        {
//            torqueOffset[k] = torques(k) - calculatedTorques[k];
//            aveTorqueOffset[k] =0;
//            if (averageTorqueOffsetQueue[k].size() > integralValue[k])
//            {
//                averageTorqueOffsetQueue[k].dequeue();
//                temTorqueOffsetQueue = averageTorqueOffsetQueue[k];
//                double sumTorque = 0 ;
//                for (int i = 0; i < averageTorqueOffsetQueue[k].size(); i++)
//                {
//                    sumTorque += temTorqueOffsetQueue.dequeue();
//                }
//                aveTorqueOffset[k] = sumTorque / averageTorqueOffsetQueue[k].size();
//            }
//           averageTorqueOffsetQueue[k].enqueue(torqueOffset[k]);
//            if( fabs(aveTorqueOffset[k] )> thresholdValue[k])
//            {
////                jointDecouple->positionCommand(k) -=proportionValue[k]*aveTorqueOffset[k];
//                //motorDataVector[0].position += 1*aveTorqueOffset;
//            }

////            qDebug()<<"0000000000000000*****************axisId" << k;
////            qDebug()<<"11111111111111111111*************torqueOffset"<<torqueOffset[k]<< "calculate gravity="
////                      <<calculatedTorques[k]<<"feedback torque="<<torques(k) << "motorFeedback torque" << motorFeedback[k].torque;
////            qDebug()<<"2222222222222222222*****************aveTorqueOffset"<<aveTorqueOffset[k];
//        }
//    }
//    jointDecouple->executeDecoupleToPulse();
    //analaysizerGravity->recordDataForAnalysize(tmpGravity);
    //analysizerTorques->recordDataForAnalysize(tmpTorques);
    return 1;
}


int McControllerInterpreter::dragFromGravity()
{
    if(false==isInitialOk)
    {
        return -1;
    }
    JntArray gravity(jointNum);
    JntArray positionIn(jointNum);
    //coordinate->getGravity(positionIn, gravity);
//    if (coordinate->getGravity(jointDecouple->positionCommand, gravity) < 0)
//    {
//        if(debugFlag)
//        {
//            qDebug()<<"McControllerInterpreter::dragFromGravity=======获取关节重力出错" << coordinate->getGravity
//                      (jointDecouple->positionCommand, gravity);
//        }
//    }
    for (int i = 0; i < jointNum; i++)
    {
        qDebug() << "/////////////////------------------gravity(" << i << ") =" << gravity(i) << "--------//////////////";
    }

    JntArray position(jointNum), velocity(jointNum), acc(jointNum), torques(jointNum);
//    jointDecouple->getFeedbackJoint(position, velocity, acc, torques);

//    for (int i = 0; i < jointNum; i++)
//    {
//        torques(i) = torques(i) * robotJoints[i].maxToeque / 1000.0;

//    }
    vector<double> tmpGravity;
    vector<double> tmpTorques;
    tmpGravity.resize(jointNum);
    tmpTorques.resize(jointNum);
    for (int i = 0; i < jointNum; i++)
    {
        tmpGravity[i] = gravity(i);
        tmpTorques[i] = torques(i);
    }
//    analaysizerGravity->recordDataForAnalysize(tmpGravity);
//    analysizerTorques->recordDataForAnalysize(tmpTorques);
    return 1;
}



bool McControllerInterpreter::isAxisZeroOk()
{

    if(false==isInitialOk)
    {
        return false;
    }
    return trajServer->isAxisZeroOk();


}

bool McControllerInterpreter::isAxisZeroOk(int axisId)
{
    if(false==isInitialOk)
    {
        return false;
    }

    return trajServer->isAxisZeroOk(axisId);


}

int McControllerInterpreter::getAxisZeroState(int axisId)
{
    return trajServer->getAxisZeroState(axisId);
}

QVector<int> McControllerInterpreter::getAbsoluteZeroMotorValue()
{
    return trajServer->getAbsoluteZeroMotorValue();
}

QVector<double> McControllerInterpreter::getJointMechanicalZeroValue()
{
    return trajServer->getJointMechanicalZeroValue();
}

int McControllerInterpreter::getCalibrateInformation(S_AXISGROUP_ZERO_INFORMATION &zeroInformationOut)
{
    return trajServer->getCalibrateInformation(zeroInformationOut);
}

int McControllerInterpreter::positionRegulatorLoop(const QVector<bool> &servoOnStatus, const QVector<MotorData> &motorFeedback,
                                                   QVector<MotorData> &motorCommandOut)
{
    positionRegulators->setServoStatus(servoOnStatus);
    QVector<double> targetPosition, feedbackPosition,  velOutut;
    feedbackPosition.resize(motorFeedback.size());
    for(int i=0;i<motorFeedback.size();i++)
    {
        feedbackPosition[i]=motorFeedback[i].position;
    }
    QVector<MotorData> tmp_motorCommand;
    QVector<E_MOTOR_CONTROL_MODE> controlModeOut;
    int tmpFlag;
    if(1==trajServer->algorithmFunctioncall(servoOnStatus,motorFeedback,tmp_motorCommand,tmpFlag,controlModeOut))
    {
        targetPosition.resize(tmp_motorCommand.size());
        for(int i=0;i<tmp_motorCommand.size();i++)
        {
            targetPosition[i]=tmp_motorCommand[i].position;
        }
        positionRegulators->loopRegulatorOnce(targetPosition,feedbackPosition,velOutut);

    }
    else
    {
        //锁没有获取成功会导致tmp_motorCommand[i] coredump
        return 0;
        positionRegulators->loopRegulatorOnce(feedbackPosition,velOutut);

    }
    QVector<bool> isValidOut;
    positionRegulators->isValid(isValidOut);
    for(int i=0;i<velOutut.size();i++)
    {
        if(i<motorFeedback.size())
        {
            MotorData tmpMotor=motorFeedback[i];
            if(isValidOut[i])
            {
                 tmpMotor.speed=velOutut[i];
//                 qDebug()<<"valid i"<<tmpMotor.speed;
            }
            else
            {
                //锁没有获取成功会导致tmp_motorCommand[i] coredump
                tmpMotor.speed=tmp_motorCommand[i].speed;//关节手动，会自动生成速度命令，导致虽然是ＡＧＶ轴，但是关节手动会动。
//                qDebug()<<"novalid i"<<tmpMotor.speed;
            }

            motorCommandOut.append(tmpMotor);
        }

    }
    return 1;
}

int McControllerInterpreter::positionAndVelocityRegulatorLoop(const QVector<bool> &servoOnStatus, const QVector<MotorData> &motorFeedback,
                                                              QVector<MotorData> &motorCommandOut)
{
    positionRegulators->setServoStatus(servoOnStatus);
    velocityRegulators->setServoStatus(servoOnStatus);
    QVector<double> targetPosition, feedbackPosition,  velOutut;
    QVector<double> feedbackVelocity,  torqueOutut;
    feedbackPosition.resize(motorFeedback.size());
    feedbackVelocity.resize(motorFeedback.size());
    for(int i=0;i<motorFeedback.size();i++)
    {
        feedbackPosition[i]=motorFeedback[i].position;
        feedbackVelocity[i]=motorFeedback[i].speed;
    }
    QVector<MotorData> tmp_motorCommand;
    QVector<E_MOTOR_CONTROL_MODE> controlModeOut;
    int tmpFlag;
    if(1==trajServer->algorithmFunctioncall(servoOnStatus,motorFeedback,tmp_motorCommand,tmpFlag,controlModeOut))
    {
        targetPosition.resize(tmp_motorCommand.size());
        for(int i=0;i<tmp_motorCommand.size();i++)
        {
            targetPosition[i]=tmp_motorCommand[i].position;
        }
        positionRegulators->loopRegulatorOnce(targetPosition,feedbackPosition,velOutut);
        velocityRegulators->loopRegulatorOnce_withPosition(targetPosition,velOutut,feedbackVelocity,torqueOutut);

    }
    else
    {
        positionRegulators->loopRegulatorOnce(feedbackPosition,velOutut);
        velocityRegulators->loopRegulatorOnce(velOutut,feedbackVelocity,torqueOutut);

    }

    for(int i=0;i<torqueOutut.size();i++)
    {
        MotorData tmpMotor=motorFeedback[i];
        tmpMotor.torque=torqueOutut[i];
        motorCommandOut.append(tmpMotor);
    }
    return 1;
}

int McControllerInterpreter::initial_ros()
{
    //initial Ros
    #ifdef D_USE_ROS
    int argc=0;
    char *argv[10];
    ros::init(argc, argv, "rosguiapi");
    QString configFile=D_TCRCONTROLLER_DATA_PATH;
    configFile+=D_ROS_CONFIG_FILE;
    configFile+="rosCommonConfig.xml";
    g_rosApi= new RosApi(configFile);
    #endif
    return 1;
}

int McControllerInterpreter::axisLosePower()
{
    return trajServer->axisLosePower();
}

int McControllerInterpreter::axisLosePower(int axisId)
{
    return trajServer->axisLosePower(axisId);
}

int McControllerInterpreter::calculateZeroOffset(std::vector<PointPro> pointList, std::vector<double> &zeroOffsetOut,
                                                 double &maxErrorOut)
{
    return trajServer->calculateZeroOffset(pointList,zeroOffsetOut,maxErrorOut);
}

int McControllerInterpreter::calibrateRobotByOffset(std::vector<double> offsetIn)
{
    return trajServer->calibrateRobotByOffset(offsetIn);
}

int McControllerInterpreter::validateZeroData()
{

    return trajServer->validateZeroData();
}

int McControllerInterpreter::calibrateRobot()
{
    return trajServer->calibrateRobot();


}

int McControllerInterpreter::calibrateRobot(int axisId)
{
   return trajServer->calibrateRobot(axisId);
}


int McControllerInterpreter::addMsg(int messageLevel, string componentName, string messageType, int messageCode, int robotIdIn,
                                     int parameter1, int parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;

    string infomationStr;
    QString tmpQStr;

    switch(messageCode)
    {
    case 5322:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("寸动失败，当前为运行状态！").toStdString();
       infomationStr = str1;
       break;
    }
    case 5321:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("算法模块初始化失败！").toStdString();
       infomationStr = str1;
       break;
    }
    case 5320:
    {
        infomationStr =  QObject::tr("算法模块：设备使用次数已经超过授权，请联系设备供应商注册！").toStdString();
        break;
    }
    case 5319:
    {
        infomationStr =  QObject::tr("算法模块：用户配置文件userInfo.xml校验错误，请联系设备供应商!").toStdString();
        break;
    }
    case 5318:
    {
        infomationStr =  QObject::tr("算法模块：用户配置文件userInfo.xml缺少节点数据，请联系设备供应商!").toStdString();
        break;
    }
    case 5317:
    {
        infomationStr =  QObject::tr("算法模块：用户配置文件userInfo.xml不存在,或者许可证文件不存在!").toStdString();
        break;
    }
    case 5316:
    {
        tmpQStr =  QObject::tr("算法模块：设备注册码无效，请联系设备供应商注册！本机硬件序列号：")
                +mathAuthority->getHardDiskSerialNumber();
        infomationStr=tmpQStr.toStdString();
        break;
    }
    case 5315:
    {
        tmpQStr =  QObject::tr("算法模块：设备使用授权已经到期，请联系设备供应商注册！本机硬件序列号：")
                +mathAuthority->getHardDiskSerialNumber();
        infomationStr=tmpQStr.toStdString();
        break;
    }
    case 5314:
    {
        tmpQStr =  QObject::tr("算法模块：设备使用授权即将到期，请联系设备供应商注册！本机硬件序列号：")
                 +mathAuthority->getHardDiskSerialNumber();
        infomationStr=tmpQStr.toStdString();
        break;
    }

    case 5312:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("位置初始化失败！").toStdString();
       infomationStr = str1;
       break;
    }
    case 5311:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("地图加载失败！").toStdString();
       infomationStr = str1;
       break;
    }
    case 5310:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("运动参数没有指明点类型！").toStdString();
       infomationStr = str1;
       break;
    }
    case 5309:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("请切换到T1或T2手动模式，再进行地图创建操作！").toStdString();
       infomationStr = str1;
       break;
    }
    case 5308:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("请切换到T1或T2手动模式，再进行手动操作！").toStdString();
       infomationStr = str1;
       break;
    }
    case 5307:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("运动到点失败，位置类型切换失败！").toStdString();
       infomationStr = str1;
       break;
    }
    case 5306:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("运动到点失败，V槽类型参数错误！").toStdString();
       infomationStr = str1;
       break;
    }
    case 5305:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("运动到点失败，激光扫描范围参数错误！").toStdString();
       infomationStr = str1;
       break;
    }
    case 5304:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("寸动失败，当前非停止或暂停状态，请先按停止后再尝试！").toStdString();
       infomationStr = str1;
       break;
    }
    case 5303:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("非手动Ｔ１模式，不允许寸动！").toStdString();
       infomationStr = str1;
       break;
    }
    case 5302:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("默认运动控制模式文件读取失败错误！").toStdString();
       infomationStr = str1;
       break;
    }
    case 5301:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("手自动模式状态切换失败，当前非暂停或停止状态").toStdString();
       infomationStr = str1;
       break;
    }
    case 5300:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("算法模块初始化失败").toStdString();
       infomationStr = str1;
       break;
    }





        default:
        {
            qDebug()<<"Coordinate::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}

int McControllerInterpreter::clearMsg(const Message message)
{
    if(false==isInitialOk)
    {
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","McControllerInterpreter", 5300, robotId);
        return -1;
    }

    return motionMessage->clearMsg(message);
}

int McControllerInterpreter::clearAllMsg()
{
    if(false==isInitialOk)
    {
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","McControllerInterpreter", 5300, robotId);
        return -1;
    }
    trajServer->clearAllMsg();//非停止状态不得清空轨迹。
//    startMotorWave();//导致cpu消耗高。
    motionForm->clearMessage();
    return motionMessage->clearAllMsg();
}


int McControllerInterpreter::checkRegisterStatus()
{

    int returnKey=mathAuthority->refreshAuthorityStatus();

    switch(returnKey)
    {
        case ENUM_TIME_GOING_PASSED:
        {
            addMsg( ENUM_MSG_REMIND,  "McControllerInterpreter", "McControllerInterpreter",  5314,  -1);
            break;
        }
        case ENUM_TIME_PASSED:
        {
            addMsg( ENUM_MSG_REMIND,  "McControllerInterpreter", "McControllerInterpreter",  5315,  -1);
            break;
        }
        case ENUM_SERIAL_UNEQUAL:
        {
            addMsg( ENUM_MSG_REMIND,  "McControllerInterpreter", "McControllerInterpreter",  5316,  -1);
            break;
        }
        case ENUM_REGISTER_FILE_NO_EXIST:
        {
            addMsg( ENUM_MSG_REMIND,  "McControllerInterpreter", "McControllerInterpreter",  5317,  -1);
            break;
        }
        case ENUM_REGISTER_FILE_NODE_ERROR:
        {
            addMsg( ENUM_MSG_REMIND,  "McControllerInterpreter", "McControllerInterpreter",  5318,  -1);
            break;
        }

        case ENUM_REGISTER_FILE_CHECK_CODE_ERROR:
        {
            addMsg( ENUM_MSG_REMIND,  "PowerManager", "PowerManager",  5319,  -1);
            break;
        }
        case ENUM_BOOT_COUNT_EXCEED:
        {
            addMsg( ENUM_MSG_REMIND,  "PowerManager", "PowerManager",  5320,  -1);
            break;
        }
    }

    return returnKey;
}
